perm filename ARM.OLD[AL,HE]3 blob
sn#508914 filedate 1980-05-07 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00080 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00010 00002 PDP11/45 ARM SERVO PROGRAM: REVISIONS
C00014 00003 CONDITIONAL ASSEMBLY FLAGS
C00019 00004 DEFINITIONS
C00023 00005 MACROS FOR ESTABLISHING RELATIVE POINTERS FOR DEVICE DATA BLOCK
C00025 00006 MACROS TO ESTABLISH POINTER TABLES INTO ARM AND HARDWARE DEVICE SERVO DATA BLOCKS
C00026 00007 POINTER TABLES FOR SERVO DATA
C00029 00008 SERVO FUNCTION ERROR CODES & BIT MASKS FOR SERVO, STATUS AND MODE WORDS
C00033 00009 ORGANIZATION OF SERVO DATA BLOCK
C00040 00010 DATA LIST STRUCTURES FOR POLYNOMIAL COEFFICIENTS AND DEVICE BLOCKS
C00044 00011 SERVO - START OF LEVEL 7 SERVO CODE
C00054 00012 ROUTINES FOR STOPPING JOINT MOTION
C00056 00013 ANGLES - CONVERTS A/D READINGS TO JOINT ANGLE AND VELOCITY
C00063 00014 FEEDBK - SERVO FEED BACK LOOP TO DRIVE THE JOINT MOTOR
C00070 00015 SCHED - SCHEDULES SERVO TO RUN AGAIN
C00072 00016 EVAL - FETCHES POLYNOMIAL COEF. AND EVALUATES FOR NEXT SERVICING TIME
C00085 00017 WIPER - SWITCHES A/D CHANNELS IF A/D OUT OF RANGE AND SERVO HAS MULTIPLE WIPERS
C00088 00018 MOTDRV,MOTSTP,BRKREL - MOTOR DRIVE ROUTINES
C00091 00019 INTARM - INITIALIZES SERVO CODE FOR FUTURE ARM MOTIONS
C00099 00020 DRIVE - INITIATES MOTIONS FOR ONE JOINT, SINGLE SEGMENT
C00104 00021 CENTER - CLOSES THE HAND FINGERS AND CENTERS THE ARM OVER ANY GRASPED OBJECT
C00114 00022 CNTRRT & CNTRLF - ON MONITOR FOR CENTER FUNCTION
C00118 00023 DATA FOR CENTER OPERATIONS
C00121 00024 WHERE - UPDATES THE CURRENT JT ANGLE AND DYNAMIC COEF FOR SERVOS
C00125 00025 MOVE - MOVES THE ARM JOINTS ALONG A PRECOMPUTED TRAJECTORY
C00131 00026 MOVE - DIAGNOSTIC MOVE INSTRUCTION WITHOUT TRAJECTORY MODIFICATION
C00138 00027 ATTSRV - ROUTINE FOR ATTACHING SERVOS
C00145 00028 RELSRV - ROUTINE FOR DETACHING SERVOS
C00148 00029 SETSET - RESETS SERVO BLOCK VARIABLES AFTER A MOVE IS COMPLETED
C00150 00030 SETTH - READS A/D AND INITIALIZES JOINT ANGLES FOR A GIVEN DEVICE BLOCK
C00159 00031 SETREF - READ A/D, SET REFERENCE VOLTAGE AND ZERO TACH VALUE READINGS
C00166 00032 STHLV7 & SRFLV7 - LEVEL 7 CODE FOR "SETTH" AND "SETREF"
C00172 00033 RUNSRV - SCHEDULES THE SERVOS OF A DEVICE TO RUN AND WAITS FOR THEIR COMPLETION
C00180 00034 DACSER - BLUE ARM DAC REFRESH LEVEL 4 ROUTINE AND ARM ERROR TRAP ROUTINE
C00183 00035 TOUCH - SCHEDULES TOUCH EVENTS
C00189 00036 CLRTCH - TAKES A SCHEDULED EVENT OFF OF THE TOUCH SENSOR EVENT LISTS
C00192 00037 SMPTCH,STPTCH - INITIATES AND TERMINATES UPDATING OF THE TOUCH SENSORS READINGS
C00194 00038 TCHLV7 - LEVEL 7 CODE FOR TOUCH SENSORS
C00201 00039 FORCE - SUBRS. USED FOR FORCE SENSING AND COMPLIANCE
C00204 00040 SETC - INITIALIZES FORCE SENSING/COMPLIANCE SYSTEM
C00208 00041 FRCSIG - INITIALIZES JOB STARTING BASED UPON A FORCE READING
C00213 00042 FRCOFF - TAKES A QUEUED JOB OFF OF THE FORCE SIGNAL LIST
C00215 00043 BISON - SETS UP BIAS FORCE OF A GIVEN MAGNITUDE AND DIRECTION
C00218 00044 BISOFF - TURNS OFF FORCE COMPLIANCE IN A SPECIFIED DIRECTION
C00222 00045 FRCLV7 - LEVEL 7 CODE FOR FORCE SENSING AND COMPLIANCE
C00237 00046 FBACK - LEVEL 5 FORCE SENSING BACKGROUND JOB
C00255 00047 DEFINITIONS FOR PROGRAMS USING WRIST SENSOR
C00256 00048 SETBAS - PROCEDURE TO SET THE BASE READINGS FOR THE FORCE WRIST
C00263 00049 WRIST - PROCEDURE TO READ AND RESOLVE THE FORCE WRIST READINGS
C00270 00050 GATHER - PROCEDURE TO COLLECT FORCE DATA
C00278 00051 FORCE SENSING ROUTINES DATA AREA
C00288 00052 SETSTF ROUTINE TO INITALIZE STIFFNESS SYSTEM
C00292 00053 KMOVE MOVES ALONG TRAJECTORY WITH STIFFNESS K
C00296 00054 KSRVO ROUTINE TO SERVICE ALL JOINTS ONCE PER TICK
C00302 00055 SS2 COMPUTES TRAJECTORY DEVIATION AND DETERMINES FORCE TO APPLY
C00306 00056 SS1 COMPUTES FORCE COMPENSATION
C00308 00057 REPS ROUTINE TO READ RAW FORCE SENSOR DATA
C00311 00058 TCI,TSI ROUTINES TO COMPUTE COMMANDED AND SENSED TORQUE IN I-TH JOINT
C00313 00059 GJTCAL,FINKTH ROUTINES TO COMPUTE TORQUE TRANSFORM AND STIFFNESS MATRICES
C00319 00060 DATA FOR STIFFNESS SERVO SYSTEM
C00323 00061 OPERATE ROUTINE TO SETUP SCREWDRIVER OR VISE OPERATION
C00328 00062 OPRLV6 ROUTINE TO OPERATE SCREWDRIVER OR VISE
C00334 00063 VARIABLES, CONSTANTS, AND TEMPORARY STORAGE AREA COMMON TO ALL SERVOS
C00336 00064 SRV01: 0 YELLOW ARM JOINT #1 DATA
C00339 00065 SRV02: 0 YELLOW ARM JOINT #2 DATA
C00342 00066 SRV03: 0 YELLOW ARM JOINT #3 DATA
C00345 00067 SRV04: 0 YELLOW ARM JOINT #4 DATA
C00348 00068 SRV05: 0 YELLOW ARM JOINT #5 DATA
C00351 00069 SRV06: 0 YELLOW ARM JOINT #6 DATA
C00354 00070 SRV07: 0 YELLOW ARM HAND SERVO DATA BLOCK
C00357 00071 SRV08: 0 BLUE ARM JOINT #1 DATA
C00360 00072 SRV09: 0 BLUE ARM JOINT #2 DATA
C00363 00073 SRV10: 0 BLUE ARM JOINT #3 DATA
C00366 00074 SRV11: 0 BLUE ARM JOINT #4 DATA
C00369 00075 SRV12: 0 BLUE ARM JOINT #5 DATA
C00372 00076 SRV13: 0 BLUE ARM JOINT #6 DATA
C00375 00077 SRV14: 0 BLUE ARM HAND SERVO DATA BLOCK
C00378 00078 SRV15: 0 VISE SERVO DATA BLOCK
C00381 00079 SRV16: 0 SCREWDRIVER SERVO DATA BLOCK
C00384 00080 DIAGNOSTIC DATA BUFFER: ORGANIZATION AND LOCAL STORAGE
C00386 ENDMK
C⊗;
; PDP11/45 ARM SERVO PROGRAM: REVISIONS
.TITLE ARM
.EVEN
;April 27, 1980:
; MOVE altered to eliminate runtime trajectory modification
;Jan 3, 1980:
; GATHER altered to pass back ID # in integer buf.
;Feb 9, 1979:
; GATHER and force gathering system made operational
;Jan. 19, 1979:
; WRIST and SETBASE made operational
;Aug. 31, 1978:
; DTERMS in BEJCZY.PAL modified to explicitly zero gravity loading for
;joint 1 and 6.
;May 11, 1978:
; Added call to where at beginning of center to permit manual positioning
;of arm prior to center. Fixed velocity error nulling bug in SERVO.
;December 6, 1977:
; Panic Button interupt service routine now returns via a new kernel
;system call, KRTI, permitting interupt returns from outside supervisor mode.
;November 6, 1977
; Added filtering to force signalling
;March 14, 1977:
; Changed Blue arm interface dac refresh routine to level 4 from 7
;and removed restriction on running only one device at a time.
;March 8, 1977:
; Re-calibrated blue arm after all dead tachs repaired and arm
;generally re-built.
;October 27, 1976:
; Changed some symbols to locals, put conditional compilation
;switches on linear calibration data and drive. Added WOBBLE.
;August 2,1976:
; Power supply drifting by as much as 4 counts, upped reference
;error tolerance to ignor for now.
;May 12, 1976:
; Adding procedures WRIST and SETBAS for reading and resolving
;the force wrist strain gage values.
;April 23, 1976:
; Bug in FEEDBACK subroutine corrected. Used to store the error
;torque ( ERRTQE ) in funny units, not oz-in and oz's. Also changed
;EVAL so that the inertia torque left in AC3 was in the proper units.
;March 25,1976:
; KV,KE,KI changed for BLUE arm, joint #2. ERRTOL changed from
;.05 to .15 for joint #2, .01 to .02 for joint #3, due mainly to a hardware
;bug in the ADC. Only odd ADC readings seem to be stable for these joints.
;March 20, 1976:
; Arm code uses new Kernel, does its own ADC reading. Order of
;execution altered so that polynomial evaluation is done before the
;pots and tachs are read and before the error feedback is computed.
;CONDITIONAL ASSEMBLY FLAGS
;MACRO FOR DEFINING UNDEFINED CONDITIONAL ASSEMBLY SWITCHES
.MACRO DSWT SWTCH,DEFULT
.IFNDF SWTCH
.PRINT /"SWTCH" ASSEMBLY SWITCH NOT DEFINED, USING DEFAULT VALUE
/
SWTCH==DEFULT
.ENDC
.ENDM
DSWT SNGSTP,0 ;SINGLE STEP MODE, DOES NOT CALL SCHEDULER OR SET DAC/ADC
DSWT DIAGY,1 ;DIAGNOSTIC MODE, JOINT VARIABLES SAVED IN BUFFER DBUF
DSWT STDALN,1 ;STAND ALONE, DOES NOT REQUIRE AL GRAPH ROUTINES
DSWT TACCAL,0 ;MODE TO CALIBRATE THE TACHOMETERS
DSWT NOYELW,1 ;NO YELLOW ARM YET, DON'T CREATE SERVO DATA BLOCKS
DSWT TIMER,0 ;SAVES EXECUTION TIME OF SERVO CODE IN ARRAY "TIMES"
DSWT ISLIN,1 ;JOINTS ASSUMED TO BE LINEAR, DON'T DO NON-LINEAR INTERP.
DSWT FRCDAT,0 ;SAVE FORCE SENSING SYSTEM DATA
DSWT SHORT,0 ;SET TO 1 ELIMINATE UNNECESSARY STUFF FOR SENSOR DIAGNOSTICS
DSWT FFING,0 ;SET TO 1 TO USE FORCE SENSING FINGERS
;HEADER SECTION FOR ASSEMBLING "ARM" FOR OVERLAYING WITH AL ROUTINES
.IFZ STDALN
.OFFSET -160000 ;Put it in the high memory
.IF1
.INSRT K1DEF.PAL[11,SYS]
.INSRT ALHEAD.PAL[AL,HE]
.ENDC
.OFFSET -340000 ;Put it in the high memory
.=ARMCOD ;START ADDRESS OF SERVO CODE
CODE$ == ARMCOD
DATA$ == ARMDAT
SPSWITCH == 0 ;Make sure we start off with everyone properly defined
DATA
.BLKW 96. ;RESERVE SPACE FOR WRIST CALIBRATION DATA
.INSRT BEJCZY.PAL[AL,HE]
.INSRT ARMSOL.PAL[AL,HE]
.INSRT ARITH.PAL[AL,HE]
DATA
PUTLOC ARMVER,VERSION ;TELL AL WHAT VERSION NUMBER THE BIN FILE IS
PUTLOC LINTARM,INTARM ;ESTABLISH POINTERS TO ROUTINES THAT AL CALLS
PUTLOC LCENTER,CENTER
PUTLOC LWHERE,WHERE
PUTLOC LMOVE,MOVE
PUTLOC LSQRTF,SQRTF
PUTLOC LEXP,EXP
PUTLOC LLOG,LOG
PUTLOC LSNCSD,SNCOS
PUTLOC LASIN,ASIN
PUTLOC LACOS,ACOS
PUTLOC LATAN2,ATAN2
PUTLOC LERRPTR,ERRPTR
PUTLOC LTHPTR,THPTR
PUTLOC LDVCPTR,DVCPTR
PUTLOC LUPDATE,UPDATE
PUTLOC LSOLVE,SOLVE
PUTLOC LDTERMS,DTERMS
PUTLOC LSETBAS,SETBAS
PUTLOC LWRIST,WRIST
PUTLOC LSETC,SETC
PUTLOC LFRCSIG,FRCSIG
PUTLOC LFRCOFF,FRCOFF
PUTLOC LBISON,BISON
PUTLOC LBISOFF,BISOFF
PUTLOC LPOTPTR,POTPTR
PUTLOC LGATHER,GATHER
PUTLOC LSETSTF,SETSTF
PUTLOC LOPERATE,OPERATE
.ENDC
;DATA POINTER DEFS. *K
IIII == .
.==NOTB11
.WORD 0
PCDBUF::.WORD 0 ;pcode buffer address
INTBUF::.WORD 0 ;integer buffer starting address
FPBUF:: .WORD 0 ;floating point buffer starting address
INTPTR::.WORD 0 ;current position of integer pointer
FPPTR:: .WORD 0 ;current position of fp buffer
INTSIZ::.WORD 0 ;size of integer buffer
FPSIZ:: .WORD 0 ;size of fp buffer
.==IIII
;DEFINITIONS
AC =%0 ;GENERAL ACCUMULATOR
BC =%1 ; " "
CC =%2 ; " "
DC =%3 ; " "
EC =%4 ; " "
DAT =%5 ;CONTAINS ADDRESS OF FIRST WORD OF SERVO DATA
FC =%5
SP =%6 ;STACK POINTER
PC =%7 ;PROGRAM COUNTER
;NUMBER REPRESENTATION OF VARIABLES AND CONSTANTS:
;
;JOINT ANGLES: DEGREES
;ANGULAR VELOCITY: DEGREES/MILLISECOND
;ANGULAR ACCELERATION: DEGREES/MILLISECOND**2
;TIME: TICKS {APPROXIMATELY 1 MILLISECOND}
;TORQUE: OZ-INCHES OR OZ
;PROCESSOR PRIORITY LEVELS CURRENTLY BEING USED:
; LEVEL 7: SERVO JOBS, FORCE SENSING TRIGGER JOB, TOUCH SENSING
; TRIGGER JOB, SETTH ADC READER, SETREF ADC READER
; LEVEL 6: NOT USED
; LEVEL 5: CENTER ON-MONITOR, FORCE SENSING AND COMPLIANCE CALCULATOR
; LEVEL 4: BLUE ARM DAC RE-FRESH JOB, FORCE SENSING ERROR MESSAGE RTN
;YELLOW INTERFACE DEFS
;DRATRP ==530 ;DR11 A VECTOR (NOT USED)
DRBTRP ==374 ;DR11 B VECTOR
DR11S =167770 ;DR11 STATUS WORD
DR11O =167772 ;DR11 OUTPUT REGISTER
DR11I =167774 ;DR11 INPUT REGISTER
PANICB =200
YDACEN =100000 ;YELLOW DAC ENABLE BIT
EMFSEN =200
;BLUE INTERFACE DEFS
DACCSR =174000 ;DAC STATUS AND CONTROL WORD ADDRESS
ADCSR =174004 ;ADC CONTROL AND STATUS WORD ADDRESS
ADCCHN =174005 ;ADC CHANNEL ADDRESS
ADCVAL =174006 ;ADC VALUE ADDRESS
DACDNE ==200 ;DAC DONE BIT
;RANDOM DEFINITIONS
DBUFL ==6000. ;LENGTH OF DIAGNOSTIC BUFFER
MAXSRV ==16. ;NUMBER OF EXISTING SERVOS
NULTME ==5000. ;NUMBER OF MILLISECONDS ALLOWED TO NULL FINAL POS. ERROR
STPTME ==1000. ; " " " " FOR ARM TO STOP AFTER AN
; ERROR HAS OCCURRED
CTRSTP ==1500. ;NUMBER OF MILLISECONDS ALLOWED FOR HAND TO CLOSE AFTER
; BOTH TOUCH SENSORS ACTIVATED IN CENTER FUNCTION
;REFERENCE POWER SUPPLY DATA
DATA
REFCN1 ==16 ;ADC CHANNELS FOR REFERENCE READING
REFCN2 ==17
REFER1: -1007. ;POWER SUPPLY READING AT TIME OF JOINT CALIBRATION
REFER2: 1021.
REFVT1: -1007. ;CURRENT POWER SUPPLY READINGS
REFVT2: 1021.
REFTOL ==6 ;ERROR TOLERANCE PERMITTED BEFORE "BAD REF." IS SIGNALLED
;MACROS FOR ESTABLISHING RELATIVE POINTERS FOR DEVICE DATA BLOCK
;INT ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 2
.MACRO INT SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+2
.ENDM
;BYTE ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 1
.MACRO BYTE SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+1
.ENDM
;REAL ASSIGNS THE SYMBOL SYM TO THE NEXT AVAILABLE RELATIVE NUMBER AND
;INCREMENTS RELCNT BY 4
.MACRO REAL SYM ;Just gives SYM the next number.
.IFDF SYM
.IF1
.ERROR You are using SYM in two ways!!!
.ENDC
.ENDC
SYM == RELCNT
RELCNT == RELCNT+4
.ENDM
;LSKIP INCREMENTS THE RELATIVE POINTER "RELCNT" BY 2*N
.MACRO LSKIP N
.REPT N
RELCNT == RELCNT+2
.ENDR
.ENDM
;MACROS TO ESTABLISH POINTER TABLES INTO ARM AND HARDWARE DEVICE SERVO DATA BLOCKS
.MACRO YELPTR LABEL
YJTPTR LABEL
SRV07+LABEL
.ENDM
.MACRO YJTPTR LABEL
SRV01+LABEL
SRV02+LABEL
SRV03+LABEL
SRV04+LABEL
SRV05+LABEL
SRV06+LABEL
.ENDM
.MACRO BLUPTR LABEL
BJTPTR LABEL
SRV14+LABEL
.ENDM
.MACRO BJTPTR LABEL
SRV08+LABEL
SRV09+LABEL
SRV10+LABEL
SRV11+LABEL
SRV12+LABEL
SRV13+LABEL
.ENDM
.MACRO HDVPTR LABEL
SRV15+LABEL
SRV16+LABEL
.ENDM
;POINTER TABLES FOR SERVO DATA
DATA
SERVOS: ;POINTERS TO SERVO LISTS, ORDER FOLLOWS BIT ASSIGNMENTS
YSRVOS: YELPTR 0 ;YELLOW ARM AND HAND JOINTS
BSRVOS: BLUPTR 0 ;BLUE ARM AND HAND JOINTS
HDEVS: HDVPTR 0 ;POINTER TO HAREWARE DEVICES: VISE AND SCREW DRIVER
;POINTERS TO ARM JOINT ANGLES
THPTR:
YTH: YELPTR TH ;POINTERS TO CURRENT YELLOW ARM JOINT ANGLES
BTH: BLUPTR TH ; " " " BLUE " " "
HTH: HDVPTR TH ; " " " HADWARE DEVICE JOINT ANGLES
THPPTR:
YTHP: YELPTR THP ; " " PREDICTED YELLOW " " "
BTHP: BLUPTR THP ; " " " BLUE " " "
;POINTERS TO RAW POT READINGS
POTPTR: YELPTR POT
BLUPTR POT
;TABLE OF POINTERS TO YELLOW ARM DYNAMIC COEFFICIENTS
YCI: YJTPTR CI ;CURRENT GRAVITY LOADING TERMS
YJTPTR CII ; " JOINT INERTIA TERMS
YNCI: YJTPTR NCI ;INITIAL GRAVITY LOADING TERMS
YJTPTR NCII ; " JOINT INERTIA TERMS
;TABLE OF POINTERS TO BLUE ARM DYNAMIC COEFFICIENTS, CI & CII
BCI: BJTPTR CI ;CURRENT GRAVITY LOADING TERMS
BJTPTR CII ; " JOINT INERTIA TERMS
BNCI: BJTPTR NCI ;INITIAL GRAVITY LOADING TERMS
BJTPTR NCII ; " JOINT INERTIA TERMS
;TABLE OF POINTERS TO ERROR TORQUE TERMS
ERRPTR: YELPTR ERRTQE ;POINTERS TO JOINT ERROR TORQUE TERMS
BLUPTR ERRTQE ; " " " " " "
;TABLE OF POINTERS TO SERVO DEVICE BLOCKS
DVCPTR: YELPTR INUSE ;POINTERS TO DEVICES TO WHICH SERVOS ARE ATTACHED
BLUPTR INUSE
HDVPTR INUSE
;TABLE OF POINTERS TO JOINT POSITION ERROR TERMS
PERPTR: BJTPTR POSERR ;POINTERS TO POSITION ERROR TERMS
;SERVO FUNCTION ERROR CODES & BIT MASKS FOR SERVO, STATUS AND MODE WORDS
;THE FOLLOWING ARE THE POSSIBLE ERROR CODES THAT CAN BE RETURNED IN R0 AFTER
;A SERVO FUNCTION HAS BEEN EXECUTED, EG. MOVE,CENTER.
CNTATT==1 ;COULD NOT ATTACH TO REQUESTED JOINT(S)
WRGNUM==2 ;INCORRECT NUMBER OF JOINTS REQUESTED TO BE DRIVEN
BADWIP==3 ;WIPERS COULD NOT BE READ WITHIN THEIR OPERATING RANGE
NOSOLU==4 ;ARM SOLUTION DOES NOT EXIST
UNKTCH==5 ;UNKNOWN TOUCH SENSOR REQUESTED
TOMTCH==6 ;NO MORE FREE SLOTS IN TOUCH SENSOR EVENT LIST
NOPOWR==7 ;ARM INTERFACE POWER SUPPLY TURNED OFF
BADREF==10 ;REFERENCE POWER SUPPLY OUT OF RANGE
BADTAC==11 ;ZERO VELOCITY TACHOMETER READING OUT OF RANGE
WRGARM==12 ;ATTEMPTED TO SWITCH ARMS WHILE FORCE SERVOING
TOMFRC==13 ;NO MORE FREE SLOTS IN FORCE SENSOR EVENT LIST
FWRGJT==14 ;NEED ALL 6 ARM JOINTS IN ORDER TO DO FORCE SENSING/COMPLIANCE
NOPOLY==15 ;CAN'T FORCE SERVO MOTION WITHOUT POLYNOMIAL
BOVERR==16 ;BACKGROUND JOB TOOK TOO LONG TO EXECUTE
;IF NO ERROR OCCURRED, A ZERO IS RETURNED. ANY OTHER NON-ZERO R0 VALUES
;CORRESPONDS TO THE STATUS WORD BIT ERROR FLAGS PRESENTED BELOW, EG. NONEX,
;ADERR.
;STATUS WORD BIT MASKS
NXTFIN ==1 ;NEXT SERVICING PERIOD WILL BE "FINAL".
FINAL ==2 ;IN FINAL STAGE OF MOTION, JUST NULLING ERRORS
;!!!!!!DONT CHANGE BIT ASSIGNMENTS OF "NXTFIN"&"FINAL"!!!!!!
RUN ==4 ;RUN SERVO, SET TO ZERO TO STOP SERVOING
MOVING ==10 ;DEVICE IS IN MOTION, I.E. THE VELOCITY IS NOT KNOWN TO BE ZERO
STROUT ==20 ;JOINT STARTED OUTSIDE OF PERMITTED OPERATING RANGE
FIRST ==40 ;FIRST PASS THRU SERVO
FSERVO ==100 ;FORCE SERVO THIS JOINT
NONEX ==400 ;JOINT IS DOWN, INOPERABLE
ADERR ==1000 ;CATASTROPHIC A/D ERROR HAS OCCURRED
PANCBT ==2000 ;PANIC BUTTON WAS PUSHED
EXJTFC ==4000 ;EXCESSIVE JOINT FORCE ERROR
TMEOUT ==10000 ;FUNCTION TOOK TOO LONG TO EXECUTE
PASLIM ==20000 ;JOINT STOPPED BEYOND STOP LIMIT
FNOSOL ==40000 ;NO ARM SOLUTION WHILE DOING FORCE COMPLIANCE
;MODE WORD BIT MASKS
NNUL ==1 ;DO NOT NULL POSITION ERROR AT THE END OF A TRAJECTORY
WOBBLE ==2 ;WOBBLE OUTER JOINTS AT END OF TRAJECTORY
DEPTPT ==4 ;MOTION HAS A DEPARTURE POINT
;BIT MASKS TO TEST FOR BLUE OR YELLOW HAND IN POLYNOMIAL BLOCK HEADER
YHAND ==1000 ;YELLOW HAND
BHAND ==4 ;BLUE HAND
;BIT MASKS TO TEST FOR BLUE OR YELLOW ARM JOINTS IN POLYNOMIAL BLOCK HEADER
YARM ==176000;YELLOW ARM
BARM ==770 ;BLUE ARM
;BIT MASKS TO TEST FOR VISE AND SCREWDRIVER IN POLYNOMIAL BLOCK HEADER
VISE ==2 ;VISE
SCREW ==1 ;SCREWDRIVER
;ORGANIZATION OF SERVO DATA BLOCK
;RELATIVE ADDRESSES FROM START OF SERVO DATA BLOCK
RELCNT ==2 ;FIRST WORD CONTAINS STATUS BITS
INT MODBTS ;OPERATION MODE BITS, EG. WOBBLE, NNUL
INT INUSE ;0 IF SERVO NOT ATTACHED, ELSE POINTER TO DEVICE BLOCK.
BYTE SRVNUM ;LOGICAL NUMBER OF SERVO
BYTE MECHSM ;BIT INDICATING WHICH MECHANISM THIS SERVO IS A PART OF(SEE BELOW)
REAL TH ;CURRENT JOINT ANGLE READ FROM POTS IN DEGREES
REAL THP ;PREDICTED JOINT ANGLE IN DEGREES
REAL THN ;PREDICTED JOINT ANGLE FOR NEXT SERVOING PERIOD, IN DEGREES
REAL THD ;CURRENT ANGULAR VELOCITY OF JOINT IN DEGREES/TICK
REAL THDP ;PREDICTED ANGULAR VELOCITY IN DEGREES/TICK
REAL THDN ;PREDICTED ANGULAR VELOCITY FOR NEXT SERVOING PERIOD, IN DEGREES
REAL DELTH ;TOTAL DEVIATION OF JOINT ANGLE DUE TO DELTHD
REAL DELTHD ;VELOCITY FACTOR FOR MODIFYING TRAJECTORY WHILE IN MOTION
REAL POSERR ;POSITION ERROR, TH-THP
INT WOBPTR ;INDEX INTO WOBBLE SINUSOIDAL TABLE, -1 FOR JTS THAT DONT WOB.
INT WOBMAG ;PTR TO MAGNITUDE OF WOBBLE IN JTS 4,5,6
INT DATPT ;PTR TO START OF POLYNOMIAL DATA LIST
INT POLY ;PTR TO POLYNOMIAL A5 COEF. FOR THIS SERVO
INT FCI ;PTR TO FINAL JOINT GRAVITY LOADING FOR SEGMENT
INT SDTIME ;MINIMUM TIME BETWEEN SCHEDULING SERVO FOR RUNNING, IN TICKS
INT TTIME ;TOTAL TIME OF PRESENT SEGMENT, IN TICKS
REAL FTTIME ; " " " " " " "
INT PTIME ;TIME INTO THE PRESENT SEGMENT WHEN WE SERVICE AGAIN, IN TICKS
REAL ELTIME ;ELAPSED TIME SINCE LAST READING IN TICKS
INT COUNT ;TIME ALLOWED FOR PRESENT FUNCTION
REAL ERRINT ;INTEGRAL OF THE POSITION ERROR
REAL ERRTQE ;TOTAL ERROR TORQUE TERM
REAL ERRTOL ;ERROR TOLERANCE FOR DETERMINING IF POS., VEL. WITHIN LIMITS
REAL KV ;VELOCITY ERROR FEEDBACK RATIO (NEG #)
REAL KE ;POSITION ERROR FEEDBACK RATIO (NEG #)
REAL KI ;INTEGRAL POSITION ERROR FEEDBACK RATIO (NEG #)
REAL V0 ;FRICTION RESISTANCE IN MOTOR TORQUE
REAL CI ;CURRENT JOINT GRAVITY LOADING TERM
REAL DCI ;CHANGE IN JOINT GRAVITY LOADING THROUGH SEGMENT
REAL NCI ;INITIAL GRAVITY LOADING AT START OF SEGMENT
REAL CII ;CURRENT JOINT INERTIA TERM
REAL DCII ;CHANGE IN JOINT INERTIA THROUGH SEGMENT
REAL NCII ;INITIAL JOINT INERTIA AT START OF SEGMENT
REAL FLEVEL ;FORCE LEVEL OUTPUT IF IN FORCE SERVO MODE
REAL KKI ;INTEGRAL FEEDBACK FOR STIFFNESS SYSTEM
REAL KKV ;VELOCITY FEEDBACK FOR STIFFNESS SYSTEM
REAL CTH0 ;ZERO POINT FOR COMPLIANT MOTION
REAL KKK ;STIFFNESS IN THIS JOINT OZ-IN/DEG
REAL KKF ;FORCE ERROR FEEDBACK COEFFICENT FOR STIFFNESS SYSTEM
REAL YOLD ;FORCE COMPENSATION OLD OUTPUT
REAL UOLD ;FORCE COMPENSATION OLD INPUT
INT POT ;A/D POSITION POT READING, UNMODIFIED
INT TACH ;A/D TACHOMETER READINGS, UNMODIFIED
INT POTCHN ;A/D POSITION POT CHANNEL
INT TACCHN ;A/D TACHOMETER CHANNEL, -1 IF NONE
INT DACADD ;DAC STATUS/CONTROL WORD ADDRESS
BYTE DACCHN ;DAC CHANNEL FOR MOTOR + TIME OUT RESTART
BYTE DACVAL ;OUTPUT TO DAC SAMPLE WORD
INT DACINF ;DAC TACH. FEEDBACK BIT IN LEFT HALF, BRAKE BIT IN RIGHT HALF
REAL MSCALE ;SCALE FACTOR TO CONVERT MOTOR TORQUE TO DAC UNITS
REAL TOTQE ;CONVERSION FACTOR FROM DEG. TO RAD. FOR PROPER UNITS OF TORQUE
INT DITHER ;MAGNITUDE OF DITHER TO BE ADDED TO JOINT TORQUE IN DAC UNITS
REAL SCALE ;SCALE FACTOR TO CONVERT FROM A/D UNITS TO DEGREES
REAL OFFSET ;OFFSET OF A/D POT READING
REAL USTOP ;UPPER LIMIT ON JOINT MOTION IN DEGREES
REAL LSTOP ;LOWER " " " " " "
REAL STPBND ;WIDTH OF STOP BAND BETWEEN USTOP OR LSTOP AND PHYSICAL STOP
REAL VSCALE ;TACH CONVERSION FACTOR FROM A/D UNITS TO DEGREES/TICK
INT MAXDRV ;MAXIMUM PERMITTED MOTOR DRIVE TORQUE
INT TACHZ0 ;A/D TACH READING AT ZERO VELOCITY
INT WIPERS ;POINTER TO MULTIPLE WIPER DATA, ZERO IF ONLY ONE WIPER
.IFZ ISLIN
INT NONLIN ;START OF 18 BYTES OF NON-LINEAR CALIBRATION DATA FOR POT
LSKIP 10
.ENDC
INT SPDBLK ;START OF PROCESS DESCRIPTOR BLOCK FOR THIS SERVO
;THE MULTIPLE WIPER TABLE FOLLOWS THE PROCESS DESCRIPTOR BLOCK. THE FORMAT
;FOR THIS TABLE IS LISTED ON THE PAGE WITH THE "WIPERS" SUBROUTINE.
;MECHANISM BIT FLAGS SET IN THE "MECHSM" BYTE VARIABLE
YELARM ==1 ;YELLOW ARM, NOT INCLUDING HAND
YELHND ==2 ;YELLOW HAND
BLUARM ==4 ;BLUE ARM, NOT INCLUDING HAND
BLUHND ==10 ;BLUE HAND
VIS ==20 ;VISE
SCRE ==40 ;SCREW DRIVER
;DATA LIST STRUCTURES FOR POLYNOMIAL COEFFICIENTS AND DEVICE BLOCKS
;
;THE FOLLOWING DESCRIBES THE REQUIRED ORGANIZATION FOR THE POLYNOMIAL
;COEFFICIENTS DATA LIST AND ASSOCIATED SERVO POINTERS:
;
; SERVO POINTERS DATA ARRAY
;
; XXXXXX TWO SERVO BIT WORDS
; XXXXXX 1 BIT FOR EACH SERVO
; BITS COMMAND MODE BITS, EG. NNUL,WOBBLE
; WOBMAG PTR TO WOBBLE MAGNITUDE CELL
; DATPT{PTR} → SEG2-. REL.POINTER TO THE NEXT SEGMENT
; TIME LENGTH OF SEGMENT IN MILLISEC
; TRANS POINTER TO LIST OF END POINT
; TRANSFORMS AND VALIDITY NUMBERS
; CODE PTR TO CODE TO BE SCHEDULED THIS SEG
; A0 F.P. POLY. COEF. FOR FIRST SERVO
; :
; A5
; :
; :
; A0
; :
; POLY{PTR}→ A5 F.P. POLY. COEF. FOR THIS SERVO
; :
; :
; A0
; :
; :
; A5
; NCI FINAL JT. GRAV. LOADING FOR 1ST JT.
; NCII FINAL JT. INERTIA FOR 1ST JOINT
; :
; FCI{PTR}→ NCI FINAL JT. GRAV. LD. FOR THIS SERVO
; NCII FINAL JT. INERTIA
; :
; SEG2: SEG3-. START OF NEXT SEGMENT
; :
; SEGN: 0 DUMMY SEGMENT, THIS SIGNALS THE
; END OF SEGMENT DATA
;
;THE NCII AND NCII TERMS ARE FILLED IN BY THE PRE-MOTION TRAJECTORY MODIFIER.
;SPACE FOR THESE TERMS MUST BE ALLOCATED BY THE CALLING INTERPRETER.
OPBITS ==4 ;RELATIVE ADDRESS OF MODE BITS, SEE PAGE 8 FOR BIT ASSIGNMENTS
WOBMG ==6 ;PTR TO WOBBLE MAGNITUDE CELL
DATST ==10 ;RELATIVE ADDRESS OF START OF SEGMENT DATA
;RELATIVE ADDRESSES OF DATA POINTED TO BY "DATPT"
;FIRST WORD CONTAINS RELATIVE POINTER TO NEXT TRAJECTORY LIST
SEGTME ==2 ;LENGTH OF TRAJECTORY SEGMENT IN TICKS
SEGTRN ==4 ;POINTS TO LIST OF END POINT TRANSFORMATIONS AND VALIDITY NUMBERS
; SEE STRUCTION OF LIST BELOW.
RNCODE ==6 ;POINTER TO CODE THAT IS TO BE SCHEDULED AT END OF THIS SEGMENT
A0COEF ==10 ;CONSTANT TERM OF FIRST POLYNOMIAL
A3COEF ==24 ;A3 TERM OF FIRST POLYNOMIAL
A5COEF ==34 ;A5 " " " "
;DEVICE BLOCK ORGANIZATION AND ASSOCIATED SERVO POINTER
;
; INUSE→ DEVICE: #NUM,0 ;NUMBER OF SERVOS STILL ACTIVE
; #EVENT ;EVENT TO BE SIGNALED WHEN ALL SERVOS DONE
; SRV01 ;LIST OF ATTACHED SERVOS
; SRV02
; :
; SRVXX
; 0 ;END OF LIST
;
;THE LOWER BYTE OF THE FIRST WORD OF THE DEVICE BLOCK IS USED TO STORE THE
;NUMBER OF SERVOS ATTACHED. THE HIGH BYTE IS USED TO SIGNAL ERROR CONDITIONS
;WHEN THE SERVOS ARE RUNNING.
; SERVO - START OF LEVEL 7 SERVO CODE
CODE
;THE ONLY ARGUMENT REQUIRED BY "SERVO" IS A POINTER TO THE SERVO BLOCK DATA
;FOR THE JOINT THAT IS TO BE SERVICED.
;
;THE "RUN" AND "MOVING" BITS IN THE SERVO STATUS WORD ARE USED TO DECIDE
;WHAT IS TO BE DONE THIS TICK. THE POSSIBLE COMBINATIONS OF RUN AND MOVING
;AND THEIR INTERPRETATIONS ARE PRESENTED BELOW:
;
; RUN MOVING INTERPRETATION
; 0 0 NO WORK TO DO, THE SERVO KILLS ITSELF.
; 0 1 STOPPING MOTION, SERVO CONTINUES TO RE-SCHEDULE ITSELF
; UNTIL THE JOINT VELOCITY IS BELOW THE ERROR TOLERANCE
; 1 0 MOTION NOT YET STARTED, SERVO CONTINUES TO RESCHEDULE ITSELF
; AND DO NOTHING UNTIL THE START WAIT COUNT {TTIME} IN
; MILLISECONDS IS LESS THAN OR EQUAL TO "PTIME".
; 1 1 JOINT RUNNING, EXECUTE SERVO CODE.
;
;THE SERVO WILL BEGIN ITS EXITING SEQUENCE IF ANY OF THE FOLLOWING EVENTS TAKES
;PLACE:
; 1. THE MOTION IS COMPLETED AND THE VELOCITY AND POSITION ERRORS HAVE BEEN
; NULLED TO WITHIN TOLERANCE.
; 2. THE VARIABLE "COUNT" HAS BEEN DECREMENTED PAST ZERO, IN WHICH CASE THE
; "TMEOUT" FLAG IS SET AND ALL OTHER SERVOS ARE SIGNALED TO STOP.
; 3. THE VARIABLE "BPANIC" IS FOUND TO BE NON-ZERO INDICATING THAT THE
; PANIC BUTTON HAS BEEN HIT.
; 4. THE SIGN BIT OF THE FIRST WORD OF THE DEVICE BLOCK IS SET INDICATING
; THAT ANOTHER SERVO HAS HAD A CATASTROPHIC ERROR OCCUR.
;
;IF AN ERROR OCCURRED WHILE THE JOINT WAS IN MOTION, THE BRAKES ARE SET AND A
;SPECIFIED AMOUNT OF TIME IS ALLOWED FOR THE JOINT VELOCITY TO GO TO ZERO
;BEFORE THE SERVO SIGNALS THE OTHER SERVOS ATTACHED TO THE DEVICE THAT IT HAS
;COMPLETED ITS MOTION.
;EXECUTION TIME: ??? MICRO SECONDS FOR COMPLETE SERVICE PERIOD
;REGISTERS USED:
;
; ALL CPU AND FLOATING POINT REGISTERS GARBAGED
;START OF LEVEL 7 CODE, CHECK IF FUNCTION HAS TIMED OUT
SERVO: SUB SDTIME(DAT),COUNT(DAT) ;DECREMENT TIME COUNT BY MIN. SAMP. INTEV.
BGT 1$ ;BRANCH IF MORE TIME LEFT
BIT #TMEOUT,(DAT) ;ARE WE STUCK IN A TIMEOUT LOOP?
BEQ .+6
BIC #MOVING,(DAT) ;YES, DONT WAIT FOR ZERO VELOCITY, JUST DIE
BIS #TMEOUT,(DAT) ;SET FUNCTION TOOK TOO LONG FLAG
JSR PC,CATERR ;SIGNAL OTHER SERVOS OF CATASTROPHIC ERROR
BR CHKSTP
;CHECK IF STOPPING OR IF ANOTHER JOINT HAS SUFFERED A FATAL ERROR
1$: BIT #RUN,(DAT) ;CHECK IF JOINT IN THE PROCESS OF STOPPING
BEQ CHKSTP ;BRANCH IF STOPPING
TST @INUSE(DAT) ;ELSE CHECK IF SOMEONE ELSE SIGNALED A CATAS. ERROR
BPL 2$ ;BRANCH IF NO ERROR SIGNALED
JSR PC,STPMVE ;ELSE SET THE BRAKES AND START EXIT SEQUENCE
BR CHKSTP
;CHECK IF THE PANIC BUTTON WAS HIT
2$: TST BPANIC ;CHECK IF THE PANIC BUTTON HAS BEEN HIT
BEQ STWAIT ;BRANCH IF OK
BIS #PANCBT,(DAT) ;ELSE INDICATE PANIC BUTTON HIT
JSR PC,CATERR ;SIGNAL OTHER JOINTS, CATASTROPHIC ERROR
;AFTER AN ERROR OCCURRED, CHECK IF JOINT MOTION STOPPED
CHKSTP: JSR PC,MOTSTP ;MAKE SURE BRAKES ARE ON
JSR PC,ANGLES ;DETERMINE CURRENT POSITION AND VELOCITY
BIT #MOVING,(DAT) ;CHECK IF SERVOING COMPLETED
BEQ SRVDNE ;KILL SERVO IF "MOVING" AND "RUN" ARE OFF
ABSF AC1 ;GET VELOCITY MAGNITUDE
CMPF ERRTOL(DAT),AC1 ;CHECK STOPPED, I.E. VEL LESS THAN ERR TOLERANCE
CFCC
BLT 1$ ;SKIP IF JOINT STILL MOVING
BIC #MOVING,(DAT) ;ELSE SIGNAL MOTION STOPPED
BR SRVDNE ;GO KILL SERVO
1$: JSR PC,SCHED ;STILL MOVING, RESCHEDULE TO CHECK AGAIN
JSR PC,WIPER ;CHECK IF POT WIPER IN RANGE
DISMS7 ;GO TO SLEEP
;IF START WAIT STATE, COMPARE DEAD BAND TIME "TTIME" TO PRESENT TIME "PTIME"
STWAIT: BIT #MOVING,(DAT) ;CHECK IF JOINT IN MOTION
BNE SERVNG ;IF IN MOTION, GO SERVO THE JOINT
CMP TTIME(DAT),PTIME(DAT) ;ELSE MUST BE IN START WAIT STATE,
; CHECK IF TIME TO START SERVOING
BLE 1$ ;IF PTIME≥TTIME, GO SERVO
JSR PC,SCHED ;ELSE, GO SCHEDULE THE SERVO AGAIN
DISMS7 ;GO TO SLEEP UNTIL ITS TIME TO REPEAT THE CHECK
1$: BIS #MOVING+FIRST,(DAT) ;INDICATE JOINT IN MOTION AND FIRST PASS
; THRU SERVO CODE
SUB TTIME(DAT),PTIME(DAT) ;CORRECT CURRENT TIME FOR DEAD BAND
JSR PC,BRKREL ;RELEASE THE JOINT BRAKE
BR FSTSRV
;WE ARE ACTUALLY SERVOING, CHECK IF WE NEED TO EVALUATE A POLYNOMIAL
SERVNG: LDF THN(DAT),AC0 ;UPDATE PREDICTED JOINT ANGLE
STF AC0,THP(DAT)
LDF THDN(DAT),AC0 ;UPDATE PREDICTED VELOCITY
STF AC0,THDP(DAT)
BIT #FINAL,(DAT) ;CHECK IF JUST NULLING ERRORS
BNE SAMP ;BRANCH IF SO
FSTSRV: JSR PC,SCHED ;RESCHEDULE THE JOINT
JSR PC,EVAL ;EVALATE TRAJECTORY POLYNOMIAL AND COMPUTE
; INERTIAL TORQUE
;SAMPLE THE A/D TO UPDATE THE CURRENT POSITION, "TH", AND VELOCITY, "THD",
;AND COMPUTE FEEDBACK CORRECTION
SAMP: JSR PC,ANGLES ;DETERMINE CURRENT POSITION AND VELOCITY
JSR PC,FEEDBK ;COMPUTE ERROR FEEDBACK TERMS
JSR PC,WIPER ;GET PROPER WIPER FOR NEXT SERVICING PERIOD
.IFNZ DIAGY
SAVDAT: MOV DBUF,EC ;SETUP DIAGNOSTIC DATA BUFFER
MOV PTIME(DAT),(EC)+ ;CURRENT TIME
TST ACTUAL ;CHECK IF ERROR READINGS TO BE SAVED
BNE SAVACT
LDF POSERR(DAT),AC0 ;SEND BACK POSITION AND VELOCITY ERROR
LDF VELERR,AC1
BR SAVRDG
SAVACT: LDF TH(DAT),AC0 ;SEND BACK ACTUAL JOINT ANGLE AND VELOCITY
LDF THD(DAT),AC1
SAVRDG: STF AC0,(EC)+
STF AC1,(EC)+
MOVB DACVAL(DAT),AC ;SAVE OUTPUT TORQUE
MOV AC,(EC)+
MOV EC,DBUF ;SAVE BUFFER POINTER FOR NEXT TIME
.ENDC
.IFNZ TIMER
MOVB CLKCNT,@TIMES ;SAVE EXECUTION TIME
INC TIMES
.ENDC
INC (DAT) ;CHANGE "NXTFIN" TO "FINAL" FLAG
BIC #NXTFIN+FIRST,(DAT)
BIT #MOVING+RUN,(DAT) ;CHECK IF SERVOING COMPLETED
BEQ SRVDNE ;KILL SERVO IF DONE
DISMS7 ;GO TO SLEEP UNTIL NEXT SERVOING PERIOD
;KILL SERVO JOB PERMENANTLY
SRVDNE: DECB @INUSE(DAT) ;CHECK IF OTHER ATTACHED SERVOS STILL ACTIVE
BGT SRVDIS ;JUST DIE IF NOT LAST ONE
ADD #2,INUSE(DAT) ;ELSE, SIGNAL TERMINATION EVENT
.IFZ SNGSTP
EVSIG @INUSE(DAT) ;SIGNAL EVENT
SRVDIS: DISMS7 ;DIE
.IFF
SRVDIS: RTS PC
.ENDC
;END OF "SERVO"
;ROUTINES FOR STOPPING JOINT MOTION
;"STPMVE" - SET BRAKES, CLEAR RUN FLAG, SET TIMEOUT COUNT, AND WAIT FOR ZERO VELO.
STPMVE: BIC #RUN,(DAT) ;INDICATE SERVO NOT RUNNING ANYMORE
MOV #STPTME,COUNT(DAT) ;TIME ALLOWED FOR JOINT TO STOP MOVING
RTS PC ;EXIT
;END OF "STPMVE"
;"STPRUN" - STOP SERVO NOW (SETS BRAKES AND CLEARS FLAGS)
STPRUN: BIC #RUN+MOVING,(DAT) ;INDICATE KILL SERVO NOW
JSR PC,MOTSTP ;SET BRAKES AND STOP MOTOR DRIVE
RTS PC
;"CATERR" - CATASTROPHIC ERROR, SIGNAL ALL SERVOS ATTACHED TO OUR DEVICE
CATERR: BIS #100000,@INUSE(DAT) ;SIGNAL FATAL ERROR OCCURRED
BIC #RUN,(DAT) ;INDICATE SERVO NOT RUNNING ANYMORE
MOV #STPTME,COUNT(DAT) ;TIME ALLOWED FOR JOINT TO STOP MOVING
RTS PC ;EXIT
;END OF "CATERR"
;ANGLES - CONVERTS A/D READINGS TO JOINT ANGLE AND VELOCITY
;LEAVES THE NEW JOINT ANGLE IN "TH" AND ACO, AND THE NEW ANGULAR VELOCITY
;IN "THD" AND AC1. IF THIS SERVO HAS NO TACHOMETER ( INDICATED BY TACCHN
;BEING ZERO ) OR IF THE TACH VALUE IS NEARLY OUT OF THE A/D READING RANGE,
;THE CURRENT AND PREVIOUS JOINT ANGLES ARE DIFFERENCED TO DETERMINE THE
;ANGULAR VELOCITY.
;REGISTERS USED:
;
; AC,BC,CC,DC GARBAGED
; "TH" LEFT IN AC0, "THD" LEFT IN AC1
;EXECUTION TIME: 130 MICRO SECONDS WITH TACH
; 110 MICRO SECONDS WITHOUT TACH
;COMPUTE JOINT ANGLE
ANGLES: BITB #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE
BEQ 3$
MOVB POTCHN(DAT),ADCCHN ;START CONVERTING POT READING
LDF TH(DAT),AC1 ;GET THE PREVIOUS JOINT ANGLE
TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV ADCVAL,AC ;GET POT READING
MOVB TACCHN(DAT),ADCCHN ;START CONVERTING TACH READING
MOV AC,POT(DAT)
ADD #4000,AC ;SHIFT POT READING TO 0 → 7777
BR 4$
3$: MOV #42,DR11S
MOV POTCHN(DAT),DR11O
LDF TH(DAT),AC1 ;GET THE PREVIOUS JOINT ANGLE
TSTB DR11S ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV DR11I,AC ;GET POT READING
MOVB TACCHN(DAT),DR11O ;START CONVERTING TACH READING
MOV AC,POT(DAT)
4$:
;ADD CORRECTION FOR NON-LINEAR POT ELEMENT
.IFZ ISLIN
MOV AC,DC ;SAVE NORMALIZED POT READING
CLR BC
ASHC #-10,AC ;GET 4 MSB TO USE AS INDEX INTO NON-LINEAR TABLE
ROR BC ;LEAVE 8 LSB AS POSITIVE TWOS COMPLEMENT NUMBER
ADD DAT,AC ;COMPUTE A POINTER INTO THE NON-LINEAR CORR. TABLE
MOVB NONLIN(AC),CC ;GET LOWER LIMIT OF CALIBRATION TABLE
MOVB NONLIN+1(AC),AC ;GET UPPER LIMIT TO INTERPOLATE
SUB CC,AC ;GET DIFFERENCE
MUL BC,AC ;INTERPOLATE
ASHC #1,AC ;GET 16 MSB
ADD CC,AC ;NOW HAVE PROPER CALIBRATION
ADD DC,AC ;ADD TO NORMALIZED POT READING
.ENDC
;CONVERT TO JOINT ANGLE
LDCIF AC,AC0 ;CONVERT TO FLOATING POINT
MULF SCALE(DAT),AC0 ;CONVERT FROM A/D UNITS TO JOINT ANGLE
ADDF OFFSET(DAT),AC0 ;ADD POT OFFSET
STF AC0,TH(DAT) ;SAVE THE JOINT ANGLE WITH THE SERVO DATA
;COMPUTE THE ANGULAR VELOCITY OF THE JOINT
.IFZ TACCAL
TST TACCHN(DAT) ;CHECK IF THERE IS A A/D TACH CHANNEL
BGT 2$ ;JUMP IF WE DO HAVE A TACHOMETER
1$: SUBF AC0,AC1 ;NO TACH, DIFFERENCE POSITION READINGS
NEGF AC1
DIVF ELTIME(DAT),AC1 ;DIVID BY ELAPSED TIME BETWEEN READINGS
STF AC1,THD(DAT) ;SAVE VELOCITY( DEG/TICK )
RTS PC ;RETURN
2$: BITB #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE
BEQ 6$
TSTB ADCSR ;WAIT TILL ADC CONVERSION DONE
BPL .-4
MOV ADCVAL,AC ;GET TACH READING
BR 7$
6$: TSTB DR11S
BPL .-4
BPT
MOV DR11I,AC
7$: SUB TACHZ0(DAT),AC ;SUBTRACT ZERO VELOCITY TACH READINGS
MOV AC,BC
BPL .+4
NEG BC
CMP #3400,BC ;CHECK IF READING OUT OF RANGE
BLE 1$ ;DIFFERENCE POSITION IF SO
BIC #7,BC ;VEL = 0 IF IN NOISE BAND
BNE .+4
CLR AC
LDCIF AC,AC1 ;CONVERT TO FLOATING POINT
MULF VSCALE(DAT),AC1 ;CONVERT FROM A/D UNITS TO DEGREES/TICK
STF AC1,THD(DAT) ;SAVE VELOCITY
RTS PC
.IFF
2$: BITB #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE
BEQ 6$
TSTB ADCSR ;WAIT TILL ADC CONVERSION DONE
BPL .-4
MOV ADCVAL,AC ;GET TACH READING
BR 7$
6$: TSTB DR11S
BPL .-4
MOV DR11I,AC ;GET TACH READING
7$: LDCIF AC,AC1 ;ACCUMULATE TACH READINGS
TST AC ;GET ABS VALUE OF READING
BGE .+4
NEG AC
ADDF TTACH,AC1
CMP #3400,AC ;CHECK IF READING OUT OF RANGE
BGT .+6
INC TACOFR ;KEEP TRACK OF NUMBER OF READINGS OUT OF RANGE
STF AC1,TTACH
INC TACRDG ;KEEP TRACK OF NUMBER OF READINGS TAKEN
CLRF AC1 ;SET ZERO VELOCITY
STF AC1,THD(DAT) ;SAVE VELOCITY
RTS PC
DATA
TTACH: .BLKW 2 ;ACCUMULATED TACH READINGS FOR TACCAL MODE
TACRDG: 0 ;NUMBER OF TACH READINGS TAKEN
TACOFR: 0 ;NUMBER OF TACH READINGS OUT OF RANGE
CODE
.ENDC
;END OF "ANGLES"
;FEEDBK - SERVO FEED BACK LOOP TO DRIVE THE JOINT MOTOR
;COMPUTES THE OUTPUT MOTOR DRIVE BY DETERMINING THE POSITION AND VELOCITY ERROR
;AND ADDING IN THE EFFECTS OF GRAVITY LOADING, FRICTION, AND INERTIAL ACCELERATION.
;THE IMPLEMENTED EQUATION IS AS FOLLOWS:
;
; MOTOR TORQUE = CII*THDDP + CI - KE*(TH-THP) - KV*CII*(THD-THDP)
; - (KI/CII)*INTEGRAL(TH-THP) + or - V0
; WHERE
; CII = JOINT INERTIA
; CI = JOINT GRAVITY LOADING
; TH = ACTUAL JOINT ANGLE
; THP = PREDICTED JOINT ANGLE
; THD = ACTUAL JOINT ANGULAR VELOCITY
; THDP = PREDICTED JOINT ANGULAR VELOCITY
; THDDP= PREDICTED JOINT ANGULAR ACCELERATION
; V0 = FRICTION COMPENSATION WITH THE SAME SIGN AS VELOCITY
; KE,KI,KV = FEEDBACK COEFFICIENTS
;
;THE INTEGRAL OF POSITION ERROR IS DEVELOPED BY SUMMING THE POSITION ERROR EACH
;TIME THE SERVO IS SERVICED.
;EXECUTION TIME: 240 MICRO SECONDS
;REGISTERS USED:
; AC0 MUST BE LOADED WITH "TH" AND AC1 "THD" PRIOR TO CALLING FEEDBK
; AND THE INERTAL TORQUE MUST BE LOADED INTO AC3 IF ON TRAJECTORY
FEEDBK: SUBF THP(DAT),AC0 ;SUBT THE PREDICTED JOINT ANGLE FROM THE ACTUAL
BIT #FINAL,(DAT) ;CHECK IF MOTION COMPLETED
BEQ CMPFBK ;BRANCH IF STILL ON TRAJECTORY
;AT END OF TRAJECTORY, NULL ERRORS AND EXIT IF WITHIN TOLERANCE
BIC #FSERVO,(DAT) ;DON'T FORCE SERVO DURING NULLING TIME
CLRF AC3 ;IGNOR JOINT INERTIAL ACCELERATION
CLRF THDP(DAT) ;PREDICTED VELOCITY MUST BE ZERO
BIT #NNUL,MODBTS(DAT);CHECK IF WE NULL ERRORS AT END OF TRAJ.
BEQ 1$
JSR PC,STPMVE ;NO NULLING, GO STOP MOVING
JSR PC,MOTSTP
BR 2$
1$: STF AC0,AC2 ;NULLING, CHECK IF WITHIN FINAL TOLERANCE
ABSF AC2 ;GET ABS( POSITION ERROR )
CMPF ERRTOL(DAT),AC2 ;COMPARE TO PERMITTED TOLERANCE
CFCC
BLT ENDTRJ ;IF ERROR TOO LARGE, USE FEEDBACK TO REDUCE IT
2$: STF AC1,AC2 ;GET ABS( VELOCITY )
ABSF AC2
CMPF ERRTOL(DAT),AC2 ;COMPARE TO PERMITTED VELOCITY TOLERANCE
CFCC
BLT ENDTRJ ;IF TOO LARGE, CONTINUE SERVOING TO SET POINT
JSR PC,STPRUN ;NOW WITHIN TOLERANCE, KILL SERVO
RTS PC
ENDTRJ: JSR PC,SCHED ;RESCHEDULE JOINT TO RUN AGAIN
;COMPUTE FEEDBACK ERROR TERMS AND CORRECT MOTOR DRIVE
CMPFBK: STF AC0,POSERR(DAT) ;SAVE THE POSITION ERROR
BIT #FSERVO,(DAT) ;FORCE SERVOING?
BEQ 1$
LDF FLEVEL(DAT),AC0 ;YES, GET COMPLIANCE LEVEL
STF AC0,ERRTQE(DAT)
BR 2$ ;JUST ADD GRAVITY LOADING AND FRICTION COMP.
1$: STF AC0,AC2
.IFZ TACCAL
SUBF THDP(DAT),AC1 ;SUBT THE PREDICTED ANGULAR VELOCITY FROM THE ACTUAL
.ENDC
.IFNZ DIAGY
STF AC1,VELERR ;SAVE THE VELOCITY ERROR
.ENDC
ADDF ERRINT(DAT),AC2 ;SUM THE INTEGRAL POSITION ERROR
STF AC2,ERRINT(DAT)
MULF KI(DAT),AC2 ;MULTIPLY INTEGRAL BY FEEDBACK COEF., KI
DIVF CII(DAT),AC2 ;FOR STABILITY, DIVID KI BY JOINT INERTIA
MULF KE(DAT),AC0 ;MULT THE POSITION ERROR BY FEEDBACK COEF.,KE
ADDF AC2,AC0 ;POSITION + INTEGRAL ERROR FEEDBACK
MULF KV(DAT),AC1 ;MULTIPLY VELOCITY ERROR BY FEEDBACK COEF., KV
MULF CII(DAT),AC1 ;FOR STABILITY, MULT KV BY JOINT INERTIA
ADDF AC1,AC0 ;TOTAL TORQUE FEEDBACK
MULF TOTQE(DAT),AC0 ;CONVERT TO UNITS OF TORQUE
STF AC0,ERRTQE(DAT) ;SAVE THE JOINT ERROR TORQUE
ADDF AC3,AC0 ;ADD THE INERTIAL TORQUE
2$: ADDF CI(DAT),AC0 ;ADD GRAVITY LOADING
;ADD IN THE EFFECTS OF THE PREDICTED FRICTIONAL RESISTANCE
BIT #FINAL,(DAT) ;CHECK IF JUST NULLING ERRORS
BNE 3$ ;DON'T LOOK AT VELOCITY IF SO
MOV THD(DAT),AC ;GET THE SIGN AND THE MSB OF ACTUAL JOINT VELOCITY
BNE 4$
BIT #FSERVO,(DAT) ;IF FORCE SERVOING POSERR INVALID
BNE 6$
3$: MOV POSERR(DAT),AC ;USE THE PREDICTED TH - ACTUAL TH IF VELOCITY ZERO
BEQ 6$ ;DONT ADD ANY FRICTION IF BOTH VEL AND POS ERROR ZERO
NEG AC
4$: BLT 5$ ;FRICTION RESISTS MOVEMENT, CHECK DIRECTION OF VEL.
ADDF V0(DAT),AC0 ;POS. VELOCITY, COMPENSATE FOR FRICT. BY ADDING V0
BR 6$
5$: SUBF V0(DAT),AC0 ;NEG. VELOCITY, " " " " SUBT. V0
6$: JSR PC,MOTDRV ;DRIVE MOTOR
RTS PC ;RETURN
;END OF "FEEDBK"
;SCHED - SCHEDULES SERVO TO RUN AGAIN
;RESCHEDULES THE SERVO WITHIN A MINIMUM OF "SDTIME" TICKS. THE FOLLOWING
;VARIABLES ARE ALSO UPDATED:
;
; PTIME ← PTIME + ELAPSED TIME TILL NEXT RUN
; ELTIME ← FLOAT( ELAPSED TIME TILL NEXT RUN )
;
;MULTIPLE SCHEDULING CALLS ARE NOT CHECKED FOR.
;EXECUTION TIME:
;REGISTERS USED:
; AC,BC,CC,DC,EC GARBAGED
; LEAVES VALUE OF ELTIME IN AC2
SCHED:
.IFZ SNGSTP
RESCH7 SDTIME(DAT) ;RE-SCHEDULE JOINT SERVO
LDCIF (SP),AC2 ;GET ACTUAL TIME TILL NEXT RUN
ADD (SP)+,PTIME(DAT);KEEP TRACK OF TIME INTO PRESENT SEGMENT
.IFF
LDCIF SDTIME(DAT),AC2 ;IF IN DIAGNOSTIC MODE, DONT SCHEDULE
ADD SDTIME(DAT),PTIME(DAT)
.ENDC
STF AC2,ELTIME(DAT) ;SAVE F.P. ELAPSED TIME
RTS PC ;RETURN
;END OF "SCHED"
;EVAL - FETCHES POLYNOMIAL COEF. AND EVALUATES FOR NEXT SERVICING TIME
;THE JOINT ACCELERATION IS COMPUTED BY DETERMINING THE CONSTANT ACCELERATION
;THAT WILL PRODUCE THE SAME CHANGE IN ANGLE BETWEEN NOW AND THE NEXT SERVICING
;PERIOD AS THE 5TH ORDER POLYNOMIAL. THE IMPLEMENTED EQUATION IS AS FOLLOWS:
;
; ACCELERATION = ( V2 - V1 )/DT
; V2 = ( P2 - P1 )/DT
; P2 = P2' + DELTHD*DT + WOBBLE
;
; WHERE P1 = PREDICTED JOINT ANGLE FOR CURRENT SERVICING PERIOD
; V1 = " " VELOCITY " " " "
; P2 = " " ANGLE FOR THE NEXT SCHEDULED PERIOD
; P2' = POLYNOMIAL JOINT " " " " " "
; DELTHD= VELOCITY DEVIATION OF RUN TIME PATH FROM POLY. VALUE
; DT = INTERVAL OF TIME UNTIL WE ARE SCHEDULED TO BE SERVICED
; WOBBLE= SINUSOIDAL WOBBLE IN JTS 4,5,6
;
;THE PREDICTED JOINT POSITION AND VELOCITY FOR THE NEXT SERVICING PERIOD ARE
;STORED IN "THN(DAT)" AND "THDN(DAT)" AND THE INERTIAL TORQUE IS LEFT IN AC3
;
;ALSO, THE CURRENT JOINT INERTIA AND GRAVITY LOADING IS DETERMINED BY
;INTERPOLATION. THE COMPUTED VALUES ARE SAVED IN CII AND CI.
;EXECUTION TIME: 320 MICRO SEC. WHILE CHANGING SEGMENTS
; 310 MICRO SEC. FOR IN MIDDLE OF SEGMENT
; +100 MICRO SEC. IF JOINT OUT OF RANGE
;REGISTERS USED:
; EXPECTS VALUE OF ELTIME IN AC2
; LEAVES INERTIAL TORQUE IN AC3
EVAL: MOV DATPT(DAT),CC ;GET POINTER TO START OF POLY. DATA LIST
BNE 1$
;NOT ON POLYNOMIAL, JUST COMPUTE CHANGE DUE TO CONSTANT VELOCITY FACTOR
CLRF AC0 ;ZERO POLYNOMIAL POSITION
LDF CII(DAT),AC3 ;USE CONSTANT JOINT INERTIA
JMP NUDGE ;GO ADD IN CONSTANT VELOCITY FACTOR
;ON POLYNOMIAL, CHECK IF END OF SEGMENT OR START OF FIRST SEGMENT
1$: MOV TTIME(DAT),AC ;GET PRESENT TOTAL SEGMENT TIME
BIT #FIRST,(DAT) ;ARE WE JUST STARTING A MOTION?
BNE FSTSEG ;GO SET FIRST SEGMENT TIME
CMP PTIME(DAT),AC ;CHECK IF PAST END OF SEGMENT
BLT EVALPY ;BRANCH IF STILL IN SAME SEGMENT
;SWITCHING TO A NEW SEGMENT SAVE DYNAMIC COEF. AND CHECK IF FINAL SEGMENT
CHKFIN: MOV FCI(DAT),BC ;GET POINTER TO GRAV. LDG. AND INERTIA
MOV (BC)+,NCI(DAT) ;FINAL GRAV. LDG. OF PAST SEG= INITIAL GRAV. LDG
MOV (BC)+,NCI+2(DAT); OF NEXT SEGMENT
MOV (BC)+,NCII(DAT) ;SAME FOR JOINT INERTIA
MOV (BC),NCII+2(DAT)
MOV RNCODE(CC),BC ;CHECK IF POSSIBLE EVENT TO SIGNAL
BEQ 2$ ;ZERO IF NONE OR ALREADY SIGNALED
MOV AC,-(SP) ;Need to save registers - Kernel will clobber them
MOV CC,-(SP)
MOV DC,-(SP)
MOV EC,-(SP)
MOV DAT,-(SP)
EVSIG BC
MOV (SP)+,DAT ;Restore registers
MOV (SP)+,EC
MOV (SP)+,DC
MOV (SP)+,CC
MOV (SP)+,AC
CLR RNCODE(CC) ;ZERO TO INDICATE SIGNALED
2$: MOV (CC),BC ;SAVE INCREMENT TO NEXT SEGMENT
ADD BC,CC ;GET THE POINTER TO THE START OF THE NEXT SEGMENT
TST (CC) ;CHECK IF FINAL SEGMENT
BNE 1$ ;BRANCH IF NOT
;FINISHED MOTION, COMPUTE FINAL POSITION AND DYNAMIC COEF.
MOV POLY(DAT),CC ;GET POINTER TO POLYNOMIAL LIST
LDF (CC),AC0 ;COMPUTE FINAL SET POINT FROM POLY, GET A5
ADDF -(CC),AC0 ; + A4
ADDF -(CC),AC0 ; + A3
ADDF -(CC),AC0 ; + A2
ADDF -(CC),AC0 ; + A1
ADDF -(CC),AC0 ; + A0
MOV FCI(DAT),CC ;GET PTR TO GRAV. LDG. AND INERTIA
LDF 4(CC),AC3 ;SET FINAL VALUE OF JOINT INERTIA
MOV (CC)+,CI(DAT) ;SET FINAL VALUE OF GRAVITY LOADING
MOV (CC),CI+2(DAT)
STF AC3,CII(DAT)
CLR DATPT(DAT) ;INDICATE NO MORE POLYNOMIALS
BIS #NXTFIN,(DAT) ;INDICATE NEXT SERVICING PERIOD IS LAST
MOV #NULTME,COUNT(DAT) ;ALLOW NULTME SECONDS TO NULL ERRORS
JMP NUDGE ;GO ADD IN CONSTANT VELOCITY FACTOR
;NOT THE FINAL SEGMENT, UPDATE POINTERS, DYNAMIC COEF, AND SEG. TIME
1$: MOV CC,DATPT(DAT) ;SAVE NEW SEGMENT POINTER
ADD BC,FCI(DAT) ;UPDATE OTHER DATA POINTERS
ADD BC,POLY(DAT)
SUB AC,PTIME(DAT) ;GET TIME INTO NEXT SEGMENT
FSTSEG: MOV SEGTME(CC),AC ;SET THE TOTAL TIME OF THE NEXT SEG
LDCIF AC,AC0 ;FLOAT THE TOTAL SEGMENT TIME
CMP PTIME(DAT),AC ;CHECK IF PAST THIS NEW SEGMENT
BGE CHKFIN
MOV AC,TTIME(DAT) ;SAVE THE NEW TOTAL SEGMENT TIME
STF AC0,FTTIME(DAT) ;SAVE THE F.P. TOTAL TIME
MOV FCI(DAT),AC ;COMPUTE CHANGE IN GRAVITY LOADING
LDF (AC)+,AC3
SUBF NCI(DAT),AC3
STF AC3,DCI(DAT)
LDF (AC),AC3 ;COMPUTE CHANGE IN INERTA
SUBF NCII(DAT),AC3
STF AC3,DCII(DAT)
;EVALUATE THE POSITION POLYNOMIAL FOR THE NEXT SERVICING PERIOD.
EVALPY: LDCIF PTIME(DAT),AC1 ;CONVERT THE PRESENT TIME TO FLOATING POINT
MOV POLY(DAT),CC ;GET ABSOLUTE ADDRESS OF POSITION POLYNOMIAL
DIVF FTTIME(DAT),AC1 ;GET THE FRACTIONAL TIME INTO THIS SEGMENT
LDF (CC),AC0 ;EVALUATE THE POSITION POLYNOMIAL, GET A5
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A4
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A3
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A2
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A1
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A0, NOW HAVE POLYNOMIAL JOINT ANGLE
;INTERPOLATE GRAVITY LOADING AND JOINT INERTIA
LDF DCI(DAT),AC3 ;GET CHANGE IN GRAVITY LOADING
MULF AC1,AC3 ; x TIME INTO SEGMENT
ADDF NCI(DAT),AC3 ; + INITIAL G.L. FOR SEGMENT
STF AC3,CI(DAT) ;SAVE NEW GRAVITY LOADING
LDF DCII(DAT),AC3 ;REPEAT FOR JOINT INERTIA
MULF AC1,AC3
ADDF NCII(DAT),AC3
STF AC3,CII(DAT) ;SAVE NEW JOINT INERTIA
;ADD IN A CONSTANT VELOCITY FACTOR TO MODIFY THE TRAJECTORY WHILE RUNNING
NUDGE: LDF DELTHD(DAT),AC1 ;LOAD THE VELOCITY FACTOR
MULF AC2,AC1 ;VELOCITY * ELAPED TIME = CHANGE IN POSITION
ADDF DELTH(DAT),AC1 ;DETERMINE THE TOTAL POSITION DEVIATION
ADDF AC1,AC0 ;MODIFY NEXT POSITION
STF AC1,DELTH(DAT) ;SAVE THE TOTAL POSITION DEVIATION
;ADD IN WOBBLE IN ANGLE IN LAST 3 JOINTS
BIT #WOBBLE,MODBTS(DAT) ;WOBBLING?
BEQ NOWOB ;NO
MOV WOBPTR(DAT),AC ;INDEX TO SINE TABLE
BLT NOWOB ;ONLY WOBBLE JTS 4,5,6
LDF WOBTAB(AC),AC1 ;COMPUTE WOBBLE SINUSOID
MULF @WOBMAG(DAT),AC1
CMP (AC)+,(AC)+ ;INDEX AND SAVE PTR
BIC #177700,AC
MOV AC,WOBPTR(DAT)
ADDF AC1,AC0 ;ADD WOBBLE TO NEW POSITION
;CHECK IF JOINT GOING BEYOND STOP LIMIT
NOWOB: BIC #PASLIM,(DAT) ;ASSUME JOINT IN RANGE
CMPF USTOP(DAT),AC0 ;COMPARE TO UPPER STOP LIMIT
CFCC
BGE 2$ ;BRANCH IF WITHIN LIMIT
BIS #PASLIM,(DAT) ;INDICATE JOINT OUT OF RANGE
BIT #FIRST+STROUT,(DAT) ;CHECK IF JOINT STARTED OUTSIDE OF RANGE
BEQ 1$ ;BRANCH IF IT WASN'T
BIS #STROUT,(DAT) ;INDICATE JOINT STARTED OUT OF RANGE
CMPF THP(DAT),AC0 ;COMPARE NEW SET POINT TO OLD
CFCC
BGE SVENTH ;ONLY PERMIT THE JOINT TO MOVE IN RANGE
LDF THP(DAT),AC0
BR SVENTH
1$: SUBF USTOP(DAT),AC0 ;COMPUTE DISTANCE BEYOND STOP LIMIT
STF AC0,AC1
ADDF STPBND(DAT),AC0 ;(TH-STOP)+(WIDTH OF STOP BAND)
DIVF AC0,AC1 ;PERCENTAGE INTO THE STOP BAND
MULF STPBND(DAT),AC1 ;NEW ANGLE BEYOND STOP LIMIT
LDF USTOP(DAT),AC0 ;NEW ANGLE IS STOP LIMIT+DISTANCE INTO STOP BAND
ADDF AC1,AC0
BR SVENTH
2$: CMPF LSTOP(DAT),AC0 ;COMPARE TO LOWER STOP LIMIT
CFCC
BLE JTINRG ;BRANCH IF WITHIN LIMIT
BIS #PASLIM,(DAT) ;INDICATE JOINT OUT OF RANGE
BIT #FIRST+STROUT,(DAT) ;CHECK IF JOINT STARTED OUTSIDE OF RANGE
BEQ 3$ ;BRANCH IF IT WASN'T
BIS #STROUT,(DAT) ;INDICATE JOINT STARTED OUT OF RANGE
CMPF THP(DAT),AC0 ;COMPARE NEW SET POINT TO OLD
CFCC
BLE SVENTH ;ONLY PERMIT THE JOINT TO MOVE IN RANGE
LDF THP(DAT),AC0
BR SVENTH
3$: SUBF LSTOP(DAT),AC0 ;SAME PROCESS AS BEYOND UPPER LIMIT
STF AC0,AC1
SUBF STPBND(DAT),AC0
DIVF AC0,AC1
MULF STPBND(DAT),AC1
LDF LSTOP(DAT),AC0
SUBF AC1,AC0
JTINRG: BIC #STROUT,(DAT) ;INDICATE JOINT NOT STARTED OUT OF RANGE
;COMPUTE JOINT INERTIAL ACCELERATION AND TORQUE
SVENTH: STF AC0,THN(DAT) ;SAVE THE PREDICTED JOINT ANGLE FOR NEXT TIME
SUBF THP(DAT),AC0 ;SUBT CURRENT JOINT ANGLE FROM NEXT PREDICTED ANGLE
DIVF AC2,AC0 ;DIVID BY ELAPED TIME
STF AC0,THDN(DAT) ;SAVE THE PREDICTED VELOCITY FOR NEXT SERVICING TIME
SUBF THDP(DAT),AC0 ;SUBT CURRENT PREDICTED VELOCITY
DIVF AC2,AC0 ;DIVID BY TIME AGAIN
MULF TOTQE(DAT),AC3 ;CONVERT DEGREES TO RADIANS IF NECESSARY
MULF AC0,AC3 ;MULT BY JOINT INERTIA AND LEAVE VALUE OF
; NEW INERTIAL TORQUE IN AC3
RTS PC ;RETURN
;END OF "EVAL"
;WIPER - SWITCHES A/D CHANNELS IF A/D OUT OF RANGE AND SERVO HAS MULTIPLE WIPERS
;OR'S A 1 INTO "OUTRGE" IF A WIPER WAS SWITCHED. ASSUMES "WIPERS" POINTS TO
;A LIST OF DATA ARRANGED AS FOLLOWS:
;
; .BLKW 0,0,0,0,0 ;BOTTOM END OF LIST
; :
; A/D CHAN # ;ONE WORD INCREASING READING
; F.P. OFFSET ;TWO WORDS ↓
; F.P. SCALE ;TWO WORDS
; WIPERS → A/D CHAN # ;ONE WORD
; F.P. OFFSET ;TWO WORDS
; F.P. SCALE ;TWO WORDS
; :
; .BLKW 0 ;TOP END OF LIST
;REGISTERS USED:
;
; AC GARBAGED, DAT REFERENCED
WIPER: MOV WIPERS(DAT),AC ;GET THE POINTER TO THE WIPER DATA
BNE .+4
RTS PC ;RETURN IF SERVO HAS ONLY ONE WIPER
CMP POT(DAT),#-2400 ;CHECK IF A/D READING OUT OF RANGE
BGT 2$ ;JUMP IF NOT TOO LOW
SUB #12,AC ;TOO LOW- POINT TO PREVIOUS WIPER DATA
1$: TST (AC) ;CHECK IF AT END OF WIPER LIST
BNE .+4
RTS PC ;RETURN IF WE CAN'T SWITCH TO A NEW WIPER
MOV AC,WIPERS(DAT) ;ELSE SAVE THE POINTER TO THE NEW WIPER DATA
MOV (AC)+,POTCHN(DAT) ;SAVE THE NEW A/D CHANNEL NUMBER
MOV (AC)+,OFFSET(DAT) ;SAVE THE NEW F.P. OFFSET VALUE
MOV (AC)+,OFFSET+2(DAT)
MOV (AC)+,SCALE(DAT) ;SAVE NEW F.P. SCALE FACTOR
MOV (AC),SCALE+2(DAT)
BIS #1,OUTRGE ;INDICATE WIPER OUT OF RANGE
RTS PC ;RETURN
2$: CMP POT(DAT),#2400 ;CHECK IF A/D READING TOO HIGH
BGE .+4
RTS PC ;RETURN IF WITHIN RANGE
ADD #12,AC ;ELSE, POINT TO THE NEXT WIPER DATA
BR 1$ ;GO SWITCH WIPERS
DATA
OUTRGE: 0 ;"WIPER" OR'S A 1 INTO THIS WORD IF POT WIPER OUT OF RANGE,
; NEVER RESET BY SERVO CODE.
CODE
;END OF "WIPER"
;MOTDRV,MOTSTP,BRKREL - MOTOR DRIVE ROUTINES
;"MOTDRV" SETS THE MOTOR DRIVE FOR CURRENT SERVO. THE OUTPUT TORQUE MUST BE
;LOADED INTO AC0 BEFORE THE CALL TO "MOTDRV". IF THE DRIVE REQUESTED IS TOO
;LARGE, "EXJTFC" IS SET IN THE STATUS WORD AND ALL SERVOS ATTACHED TO THE SAME
;DEVICE AS THIS JOINT ARE STOPPED. THE FINAL DESIRED MOTOR TORQUE IS THE
;SUM OF THE VALUE PLACED IN AC0 BEFORE THIS ROUTINE WAS CALLED AND A DITHER
;TERM.
;EXECUTION TIME: 40 MICRO SEC. IF NO CATASTROPHIC ERROR
MOTDRV: MULF MSCALE(DAT),AC0 ;CONVERT THE MOTOR TORQUE TO A/D UNITS
NEG DITHER(DAT) ;COMPLEMENT THE JOINT DITHER TERM
STCFI AC0,CC ;CONVERT THE MOTOR DRIVE TO INTEGER
ADD DITHER(DAT),CC ;ADD DITHER TO JOINT TORQUE
MOV CC,DC ;GET ABSOLUTE VALUE
BGE .+4
NEG DC
CMP MAXDRV(DAT),DC ;CHECK IF DRIVE TOO LARGE
BGE 1$ ;BRANCH IF WITHIN RANGE
BIS #EXJTFC,(DAT) ;INDICATE EXCESSIVE JOINT FORCE
JSR PC,CATERR ;CATASTROPHIC ERROR, GO STOP ALL RELATED SERVOS
BR MOTSTP ;STOP MOTION OF THIS JOINT
1$: MOVB CC,DACVAL(DAT) ;SET DAC OUTPUT VALUE
.IFZ SNGSTP
MOV DACCHN(DAT),@DACADD(DAT) ;SEND TO DAC
.ENDC
RTS PC ;RETURN
;"BRKREL" RELEASES THE BRAKE FOR THE MOTOR BY CLEARING THE BRAKE BIT IN THE DAC REG
;EXECUTION TIME:
BRKREL: MOV DACADD(DAT),AC ;GET THE DAC ADDRESS
.IFZ SNGSTP
BISB DACINF(DAT),2(AC) ;CLEAR THE BRAKE BIT
.ENDC
RTS PC ;RETURN
;"MOTSTP" RESETS THE MOTOR DRIVE AND SETS THE BRAKE
;EXECUTION TIME:
MOTSTP: MOV DACADD(DAT),AC ;GET THE DAC ADDRESS
CLRB DACVAL(DAT) ;ZERO DAC OUTPUT
.IFZ SNGSTP
MOV DACCHN(DAT),(AC)+ ;SET A ZERO MOTOR DRIVE
BICB DACINF(DAT),(AC) ;SET THE BRAKE
.ENDC
RTS PC
;END OF MOTOR DRIVE FUNCTIONS
; INTARM - INITIALIZES SERVO CODE FOR FUTURE ARM MOTIONS
;THIS ROUTINE SHOULD BE CALLED AT LEAST ONCE BEFORE ANY ARM MOTIONS ARE
;INITIATED. THE FOLLOWING OPERATIONS ARE PERFORMED:
;
; 1) THE PANIC BUTTON INTERRUPT VECTOR IS SETUP TO SET THE VARIABLE
; "BPANIC" NON-ZERO WHENEVER THE PANIC BUTTON IS HIT WHILE ENABLED.
; 2) THE REFERENCE POWER SUPPLY VOLTAGE IS READ AND ITS VALUE IS
; COMPARED TO ITS VALUE AT THE TIME THE ARM WAS CALIBRATED. IF A
; SIGNIFICANT DISCREPENCY EXISTS, A ERROR CODE IS RETURNED.
; 3) A ZERO VELOCITY TACHOMETER READING IS TAKEN FOR ALL OPERATIONAL
; JOINTS WITH TACHS. THE ZERO READING IS STORED IN THE "TACHZ0"
; WORD OF THE SERVO DATA BLOCKS. IF A READING DEVIATES BY MORE
; THAN 10 ADC COUNTS FROM THE INITIAL "TACHZ0" VALUE, AN ERROR
; IS SIGNALED.
; 4) THE CURRENT JOINT ANGLE FOR ALL OPERATIONAL JOINTS IS READ AND
; SAVED IN THE SERVO DATA BLOCKS. AT THE SAME TIME, THE GRAVITY
; LOADING AND INERTIA CONSTANT FOR EACH JOINT IS COMPUTED.
; 5) THE SERVO DATA BLOCKS FOR ALL OPERATIONAL JOINTS ARE INITIALIZED.
; 6) FORCE EVENT LIST IS INITIALIZED AND ALL FORCE SENSING IS
; TERMINATED.
;
;THIS ROUTINE REQUIRES AS ITS ONLY ARGUMENT THE ADDRESS OF A DEVICE BLOCK WHICH
;IS AT LEAST 2+N WORDS LONG, WHERE N IS THE NUMBER OF JOINTS CURRENTLY IN
;EXISTANCE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #DEVICE,R1 ;DEVICE STORAGE BLOCK
; JSR PC,INTARM
; TST R0 ;CHECK FOR ERROR CONDITIONS
;
;AT THE END OF EXECUTION, REGISTER R0 RETURNS THE ERROR CODE GENERATED BY THIS
;ROUTINE. THE INTERPRETATION OF THIS ERROR CODE IS GIVEN ON PAGE 8 OF THIS
;PROGRAM.
;REGISTERS USED:
; R0 AND R1 PASS ARGUMENTS AND R0 IS ALTERED
; ALL FLOATING AC'S ARE GARBAGED
INTARM: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
;SETUP BUTTON INTERRUPT HANDLER ROUTINE
.IFZ SNGSTP
SETVEC #ARMTRP,#ARMERR,#SUPST7 ;SET PANIC BUTTON TRAP ROUTINE
SETVEC #DRBTRP,#YERR,#SUPST7 ;SET YELLOW PANIC BUTTON TRAP ROUTINE
CLR KICDAC ;INDICATE NO DAC REFRESH JOB RUNNING
.ENDC
;RANDOM INITIALIZATION
CLR ADCSR ;TURN OFF BLUE ADC INTERRUPTS
;; CLR DR11S ;CLEARS TIMEOUT *K
;; CLR DR11O
;; BIS #3,DR11S ;CLEARS INTERUPT REQUEST
;; TST DR11I ;(ALSO CLEARS PANIC BIT)
; MOV #1,DR11S ;ENABLE YELLOW PANIC INTERUPT
; MOV #4000,DR11O
CLR ACTNUM ;RESET TOUCH SENSOR EVENT DATA
MOV #TCHVAL,R3
MOV #LSTUSE-TCHVAL/2,R4
CLR (R3)+
SOB R4,.-2
;INITIALIZE FORCE SENSING/COMPLIANCE ROUTINES
MOV #FRCJOB,R2 ;FREE ALL FORCE JOB BLOCKS
MOV R2,FRCPTR ;FORM ONE BIG CHAIN OF LINKED BLOCKS
MOV #FRCBLK,R4 ;NUMBER OF BLOCKS TO LINK
1$: MOV R2,R3 ;LINK THEM
ADD #16,R2 ;EACH BLOCK IS 7 WORDS LONG
MOV R2,(R3)
SOB R4,1$
CLR (R3) ;ZERO MARKS END
MOV #FZAPS,R2 ;ZERO ALL REQUIRED FORCE WORDS
MOV #FZAPE-FZAPS/2,R3
CLR (R2)+
SOB R3,.-2
;CLEAR THE LOCKS ON NON-REENTRANT CODE
MOV #-1,STTLCK ;SETTH
MOV #-1,SRFLCK ;SETREF
MOV #-1,BASLCK ;SETBAS
MOV #-1,WSTLCK ;WRIST
CLR BOVLCK ;BOVERM LOCK
;CHECK WHICH JOINT SERVOS ARE OPERATIONAL
MOV #MAXSRV,R3 ;GET NUMBER OF EXISTING SERVOS TO CHECK
MOV #SERVOS,R0 ;GET PTR TO LIST OF EXISTING SERVOS
CLR R4 ;CONSTRUCT SERVO BITS IN HERE
CLR R5
2$: MOV (R0)+,R2 ;GET POINTER TO SERVO DATA BLOCK
ASHC #1,R4 ;MAKE ROOM FOR NEXT SERVO BIT
BIT #NONEX,(R2) ;CHECK IF JOINT OPERATIONAL
BNE 3$ ;BRANCH IF ITS NOT
BIS #1,R5 ;ELSE SET SERVO BIT FOR FURTHER CHECKING
JSR PC,SETSET ;INITIALIZE SERVO DATA VARIABLES
3$: SOB R3,2$ ;REPEAT FOR ALL EXISTING SERVOS
ASHC #32.-MAXSRV,R4 ;LEFT JUSTIFY SERVO BITS
MOV R4,INCOEF ;SAVE SERVO BITS IN LOCAL COEFFICENT LIST
MOV R5,INCOEF+2
;ATTACH REQUIRED SERVOS
MOV #INTBLK,R0 ;POINT TO PTR TO SERVO BIT LIST
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE AND SET UP
; DEVICE BLOCK
BCS INTDNE ;BRANCH IF ATTACH ERROR OCCURRED
TST R0 ;BRANCH IF AT LEAST ONE SERVO EXISTS
BGT 4$
MOV #WRGNUM,R0 ;ELSE SIGNAL ERROR
BR INTDNE
;READ CURRENT JOINT ANGLES, TACH ZERO, AND REFERENCE POWER SUPPLY
4$: JSR PC,SETREF ;CHECK REFERENCE POWER SUPPLY VOLTAGE AND
; ZERO VELOCITY TACH READINGS
BCS INTDNE ;BRANCH IF ANY ERROR OCCURRED
JSR PC,SETTH ;READ CURRENT JOINT ANGLES IN ALL JOINTS
BCS INTDNE ;BRANCH IF WIPER ERROR OCCURRED
CLR R0 ;INDICATE NO ERRORS
;EXIT SEQUENCE
INTDNE: JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE THE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;RETURN
;INFORMATION FOR SERVO DATA BLOCKS
DATA
INTBLK: INCOEF ;POINTER TO COEF. DATA LIST
0 ;NOT A ARM MOTION FUNCTION
0
0
0
0
;LOCAL STORAGE
BOVLCK: .WORD 0 ;FLAG TO SIGNAL OVERRUN CONDITION
INCOEF: .WORD 0,0,0 ;SHORT COEFFICENT LIST USED TO ATTACH SERVOS
CODE
;END OF "INTARM"
;DRIVE - INITIATES MOTIONS FOR ONE JOINT, SINGLE SEGMENT
;A DIFFERENTIAL CHANGE IN JOINT ANGLE IS INITIATED FOR A SINGLE SERVO. THIS
;MODE IS INTENDED PRIMARY AS A DIAGNOSTIC TOOL TO CALIBRATE THE JOINT VARIABLES.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 4 WDS.
; JSR PC,DRIVE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE BLOCK.
;THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS SUCCESS-
;FULLY ATTECHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE THIRD WORD OF
;THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE LOGICAL NUMBER OF
;ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN THAT SERVOS STATUS
;ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; ALL FLOATING AC'S ARE GARBAGED
.IFNZ DIAGY
DRIVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
;ATTACH REQUIRED SERVO
MOV R0,DRVBLK ;SAVE POINTER TO COEFFICIENT DATA LIST
MOV R0,R4
MOV #DRVBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE,SET UP IN DEVICE
; BLOCK
BCS DRVERR ;BRANCH IF ATTACH ERROR OCCURRED
CMP #1,R0 ;CHECK THAT ONLY ONE JOINT HAS BEEN SELECTED
BEQ 1$ ;BRANCH IF ONLY ONE
MOV #WRGNUM,R0 ;SIGNAL ERROR
BR DRVERR ;EXIT
;CORRECT TRAJECTORY FOR INITIAL JOINT ANGLE
1$: ADD #DATST+A0COEF,R4;POINT TO CONSTANT TERM OF FIRST POLYNOMIAL
MOV 4(R1),R2 ;GET POINTER TO JOINT THAT IS TO BE SERVOED
MOV TH(R2),(R4) ;SET INITIAL VALUE OF POLY TO PRESENT JOINT ANGLE
MOV TH+2(R2),2(R4)
;CHECK IF FINAL JOINT ANGLE OUT OF RANGE
MOV #5,R3 ;ADD ALL SIX COEFFICIENTS
LDF (R4)+,AC0 ;LOAD CONSTANT TERM
ADDF (R4)+,AC0 ;ADD IN ALL OTHER COEFFICIENTS
SOB R3,.-2
CMPF USTOP(R2),AC0 ;COMPARE FINAL ANGLE TO JOINT UPPER STOP LIMIT
CFCC
BGE 3$ ;SKIP IF WITHIN RANGE
2$: MOV #4,R0 ;SIGNAL ERROR
BR DRVERR ;EXIT CLEANLY
3$: CMPF LSTOP(R2),AC0 ;COMPARE FINAL ANGLE TO JOINT LOWER STOP LIMIT
CFCC
BGT 2$ ;BRANCH IF NOT IN RANGE
;INITIATE SERVOS TO RUN
.IFNZ DIAGY
MOV #DBUF+2,DBUF ;SETUP DIAGNOSTIC BUFFER
.ENDC
.IFNZ TACCAL
CLR TTACH ;CLEAR ACCUMULATED TACH READING IF TACCAL MODE
CLR TTACH+2
CLR TACRDG ;CLEAR TOTAL NUMBER OF TACH READINGS TAKEN
CLR TACOFR ;ZERO TOTAL NUMBER OF READINGS OUT OF RANGE
MOV #BELL,SG
JSR PC,TYPSTR
.ENDC
MOV DRVBLK,R0 ;PTR TO COEF. LIST
JSR PC,RUNSRV ;GO RUN THE SERVOS
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
DRVERR: JSR PC,RELSRV ;RELEASE SERVOS IN DEVICE BLOCK
MOV (SP)+,R4 ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;INFORMATION FOR SERVO DATA BLOCKS
DATA
DRVBLK: 0 ;POINTER TO COEF. DATA LIST
30000. ;ALLOW 30 SECONDS FOR FUNCTION
48. ;DELAY STARTING BY 48 MILLISECONDS
DATST ;POINTER TO START O F SEGMENT DATA
CODE
.ENDC
;END OF "DRIVE"
;CENTER - CLOSES THE HAND FINGERS AND CENTERS THE ARM OVER ANY GRASPED OBJECT
;THE ORGANIZATION OF THE COEFFICIENT DATA ARRAY MUST BE AS FOLLOWS:
;
; COFLST: XXXXXX ;TWO SERVO BIT WORDS, 7 BITS MUST BE ON, A HAND
; XXXXXX ; SERVO AND ALL JOINT SERVOS OF THE SAME ARM
; 0 ;NO COMMAND BITS
; 0 ;DONT WOBBLE
; 0 ;NO NEXT SEGMENT
; 0 ;NO FUNCTION TIME
; 0 ;NO TRANSFORM
; CODE ;PTR TO CODE TO BE SCHEDULED THIS SEG
; ;NO POLYNOMIAL TO FOLLOW
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 33 WRDS
; JSR PC,CENTER
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE BLOCK.
;THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS SUCCESS-
;FULLY ATTECHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE THIRD WORD OF
;THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE LOGICAL NUMBER OF
;ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN THAT SERVOS STATUS
;ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; AC0,AC1,AC2,AC3,AC4,AC5 GARBAGED
CENTER: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R1,-(SP) ;SAVE POINTER TO DEVICE BLOCK
;UPDATE CURRENT POSITION
MOV R0,-(SP) ;SAVE R0
MOV #CWRE,R0 ;POINT TO COEFF. LIST FOR WHERE
MOV #CDVL,R1 ;POINT TEMPORARY DEVICE LIST
JSR PC,WHERE
TST R0
BEQ 3$ ;BRANCH IF NO WHERE ERROR
3$: MOV (SP)+,R0
MOV (SP),R1 ;RESTORE R1
;ATTACH REQUIRED SERVOS
MOV R0,CNTBLK ;SAVE POINTER TO COEF. DATA LIST
MOV #CNTBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC .+6
JMP CNTDNE ;BRANCH IF ATTACH ERROR OCCURRED
CMP #7,R0 ;CHECK THAT EXACTLY 7 JOINTS ATTACHED
BEQ 1$
MOV #WRGNUM,R0 ;SIGNAL ERROR IF NOT THE RIGHT NUMBER OF JTS
JMP CNTDNE
;COMPUTE THE CHANGE IN JOINT ANGLES FOR A MOTION ALONG THE SWEEP DIRECTION
1$: MOV #YELARM,R2 ;ASSUME YELLOW ARM ATTACHED
BIT #YHAND,@CNTBLK ;CHECK WHICH HAND THIS IS
BNE .+6 ;SKIP IF IT IS THE YELLOW HAND
MOV #BLUARM,R2 ;ELSE INDICATE BLUE ARM
MOV #THPPTR,R1 ;GET POINTERS TO ALL JOINT ANGLES
MOV #CNTTRN,R0 ;STORE CURRENT ARM TRANSFORM HERE
JSR PC,UPDATE ;COMPUTE ARM TRANSFORM
LDF CNTTRN+12.,AC0 ;MODIFY THE POSITION BY THE ORIENTATION UNIT VECTOR
ADDF CNTTRN+36.,AC0
STF AC0,CNTTRN+36.
LDF CNTTRN+16.,AC0
ADDF CNTTRN+40.,AC0
STF AC0,CNTTRN+40.
LDF CNTTRN+20.,AC0
ADDF CNTTRN+44.,AC0
STF AC0,CNTTRN+44.
MOV #CNTTRN,R0 ;POINT TO NEW TRANSFORM
MOV #THPPTR,R1 ;PUT NEW JOINT ANGLES IN HERE
JSR PC,SOLVE ;COMPUTE NEW JOINT ANGLES FOR CHANGE
TST R0 ;CHECK IF SOLUTION EXISTS
BEQ 2$ ;BRANCH IF IT DOES
MOV #NOSOLU,R0 ;ELSE SIGNAL ERROR
BR CNTDNE
;SET THE CHANGE IN JOINT ANGLE IN ALL ARM JOINTS
2$: MOV #BSRVOS,R0 ;GET POINTER TO BLUE ARM SERVOS
BIT #BLUARM,R2 ;CHECK WHICH ARM WE ARE USING
BNE .+6
MOV #YSRVOS,R0 ;REPLACE WITH POINTER TO YELLOW ARM IF NECESSARY
MOV #7,R3 ;WANT TO SET DELTHD FOR ALL SIX JOINTS AND HAND
LDF JTRATE,AC1 ;LOAD THE % CHANGE IN JT ANGLE PER MILLISECOND
BR SETCTR
CNTLP: LDF THP(R1),AC0 ;LOAD FINAL PLANNED JOINT ANGLE
SUBF TH(R1),AC0 ;GET TOTAL DIFFERENTIAL CHANGE
MOV TH(R1),THP(R1) ;RESTORE PREDICTED POSITION
MULF AC1,AC0 ;GET CHANGE PER MILLISECOND
MOV TH+2(R1),THP+2(R1)
STF AC0,DELTHD(R1) ;SET DIFFERENTIAL VELOCITY
SETCTR: MOV (R0)+,R1 ;GET POINTER TO ARM SERVO DATA BLOCK
MOV TH(R1),DELTH(R1) ;INITIAL JOINT ANGLE SET TO PRESENT ANGLE
MOV TH+2(R1),DELTH+2(R1)
LDF NCI(R1),AC0 ;SET GRAVITY LOADING AND INERTIA EQUAL TO
STF AC0,CI(R1) ; INITIAL VALUES
LDF NCII(R1),AC0
STF AC0,CII(R1)
SOB R3,CNTLP ;DO ALL JOINTS
MOV #32.,TTIME(R1) ;ONLY DELAY STARTING HAND BY 32 MILL. SECONDS
MOV RATE1,DELTHD(R1) ;SET INITIAL RATE OF HAND CLOSING
MOV RATE1+2,DELTHD+2(R1)
;INITIATE ON MONITOR AND START SERVOS RUNNING
MOV #CTRBLU,R3 ;GET PTR TO BLUE ARM TOUCH SENSOR DATA BLOCK
BIT #BLUARM,R2 ;CHECK WHICH ARM WE ARE USING
BNE .+6
MOV #CTRYEL,R3 ;REPLACE WITH PTR TO YELLOW TOUCH SENSORS IF NEC
MOV (SP),CTRDVS(R3) ;SAVE PTR TO DEVICE BLOCK
CLR NUMTCH(R3) ;BOTH TOUCH SENSORS OFF TO BEGIN WITH
.IFZ SNGSTP
EVMAK ;CREATE AN EVENT FOR THE RIGHT TOUCH SENSOR
MOV (SP),RTTEVT(R3) ;SAVE EVENT NUMBER
FORK RTTPDB(R3),#CNTRRT,#USRDM;FIRE UP RIGHT T.S. ON MONITOR, LEVEL 5
.ENDC
MOV (SP),R0 ;SCHEDULE SIGNALING OF EVENT WHEN TOUCH SENSOR ON
MOV (R3),R1 ;DATA FOR RIGHT TOUCH SENSOR
JSR PC,TOUCH ;SCHEDULE EVENT SIGNALLING
.IFZ SNGSTP
EVMAK ;REPEAT FOR LEFT TOUCH SENSOR
MOV (SP),LFTEVT(R3) ;SAVE LEFT EVENT NUMBER
FORK LFTPDB(R3),#CNTRLF,#USRDM;FIRE UP LEFT T.S. ON MONITOR, LEVEL 5
.ENDC
MOV (SP),R0 ;SCHEDULE SIGNALING OF EVENT WHEN TOUCH SENSOR ON
MOV LFTCHS(R3),R1
JSR PC,TOUCH
MOV CNTBLK,R0 ;PTR TO COEF. LIST
MOV 4(SP),R1 ;PTR TO DEVICE BLOCK
JSR PC,RUNSRV ;GO RUN THE SERVOS
MOV R0,R2 ;SAVE RUN TIME ERROR CODES
.IFZ SNGSTP
MOV (SP),R0 ;CLEAR ALL HANGING EVENTS
MOV LFTCHS(R3),R1 ;TAKE EVENT OFF TOUCH LIST
JSR PC,CLRTCH
EVKIL ;DELETE EVENT NUMBER AND KILL ANY ATTACHED PROCESS
MOV (SP),R0 ;REPEAT FOR RIGHT TOUCH SENSOR
MOV (R3),R1
JSR PC,CLRTCH
EVKIL
.ENDC
MOV R2,R0 ;RESTORE RUN TIME ERROR CODES
CMP #2,NUMTCH(R3) ;CHECK IF CENTER FUNCTION EXEC. PROPERLY
BNE CNTDNE ;EXIT IF NOT BOTH TOUCH SENSORS ACTIVIATED
MOV (SP),R1 ;ELSE CLEAR "TMEOUT" BIT FOR HAND, THIS IS
MOV 20(R1),R1 ; THE NORMAL END CONDITION
BIC #TMEOUT,(R1)
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
CNTDNE: MOV (SP)+,R1 ;RESTORE POINTER TO DEVICE BLOCK
JSR PC,RELSRV ;RELEASE ATTACHED SERVOS
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;LOCAL STORAGE AREA FOR "CENTER"
DATA
CNTTRN: .BLKW 24. ;STORAGE AREA FOR ARM TRANSFORM
;INFORMATION FOR SERVO DATA BLOCKS
CNTBLK: 0 ;POINTER TO COEF. DATA LIST
15000. ;ALLOW 15 SECONDS FOR FUNCTION
16000. ;DELAY STARTING BY ALL JOINTS BUT HAND UNTIL TOUCH
0 ;NO POLYNOMIALS
CWRE: 774 ;WHERE ARE ALL 7 JOINTS OF BLUE ARM *K
0
CDVL: .BLKW 20 ;STORAGE FOR WHERE
CODE
;END OF "CENTER"
;CNTRRT & CNTRLF - ON MONITOR FOR CENTER FUNCTION
;THIS ROUTINE IS SCHEDULED FOR EXECUTION WHENEVER EITHER OF THE TOUCH SENSORS
;ON AN ARM PERFORMING A "CENTER" FUNCTION INITIALLY SWITCHES TO THE "ON"
;STATE. IF ONLY ONE TOUCH SENSOR IS ON, THE SPEED AT WHICH THE HAND IS CLOSING
;IS REDUCED AND THE JOINT SERVOS OF THE ARM START RUNNING. THE GROSS MOTION
;OF THE ARM IS INTENDED TO NEGATE THE SPEED AT WHICH THE HAND IS CLOSING TO
;PRODUCE A NET VELOCITY OF ZERO FOR THE TOUCH SENSOR IN CONTACT WITH AN OBJECT.
;IF BOTH TOUCH SENSOR ARE NOW ON, ARM JOINT MOTION IS STOPPED AND THE HAND IS
;PERMITTED TO CLOSE EVENLY ABOUT THE OBJECT. THIS ROUTINE HAS TWO ENTRY POINTS
;WHICH ARE USED FOR DIFFERENTIATING BETWEEN TOUCH SENSORS. IT IS ASSUMED THAT
;WHEN EITHER OF THESE ROUTINES IS CALLED, REGISTER R5 CONTAINS A POINTER TO
;EITHER THE "CTRYEL" OR THE "CTRBLU" DATA BLOCKS.
;ENTRY POINT FOR TOUCH SENSOR ON RIGHT SIDE OF HAND
CNTRRT: MOV #100000,R3 ;INDICATE RT. T.S. ENTRY TAKEN
MOV RTTEVT(R5),-(SP) ;PUT EVENT NUMBER ON STACK
BR MONSTR
;ENTRY POINT FOR TOUCH SENSOR ON LEFT SIDE OF HAND
CNTRLF: CLR R3 ;INDICATE LEFT T.S. ENTRY TAKEN
MOV LFTEVT(R5),-(SP) ;PUT EVENT NUMBER OF STACK
;COMMON CODE FOR BOTH ENTRY POINTS
MONSTR:
.IFZ SNGSTP
EVWAIT ;WAIT TILL THIS EVENT IS TRIGGERED
BCS MONDNE ;JUST DIE IF EVENT WAS KILLED INSTEAD OF SIGNALED
.ENDC
MOV CTRDVS(R5),R1 ;GET PTR TO DEVICE BLOCK
ADD #4,R1 ;GET PTR TO FIRST SERVO DATA BLOCK ATTACHED
MOV #6,R2 ;FIX UP 6 ARM JOINTS
TST NUMTCH(R5) ;CHECK IF THIS IS THE FIRST TOUCH SENSOR ON
BNE 2$ ;IF SECOND TOUCH, GO STOP JOINT MOTION
;ONE TOUCH SENSOR ON, START JOINT MOTION
1$: MOV (R1)+,R0 ;GET PTR TO JOINT SERVO
XOR R3,DELTHD(R0) ;INSURE JOINT GOING IN PROPER DIRECTION
MOV PTIME(R0),TTIME(R0) ;START JOINT MOVING
SOB R2,1$ ;REPEAT FOR ALL ATTACHED JOINTS
MOV (R1)+,R0 ;GET PTR TO HAND SERVO
MOV RATE2,DELTHD(R0) ;SLOW DOWN RATE OF HAND CLOSING
MOV RATE2+2,DELTHD+2(R0)
BR 3$ ;INDICATE 1 TOUCH SENSOR ON
;BOTH TOUCH SENSORS ON, STOP JOINT MOTION
2$: MOV (R1)+,R0 ;GET PTR TO JOINT SERVO
BIC #RUN,(R0) ;TELL JOINT TO STOP MOTION
SOB R2,2$ ;REPEAT FOR ALL ARM JOINTS, NO HAND
MOV (R1)+,R0 ;ALLOW HAND TO CLOSE FOR A FEW MORE MIL-SEC.
MOV #CTRSTP,COUNT(R0)
3$: INC NUMTCH(R5) ;INCREMENT THE NUMBER OF TOUCH SENSORS ON
;EXIT CLEANLY
MONDNE:
.IFZ SNGSTP
DISMISS ;ALL DONE
.IFF
JMP RUG
.ENDC
;END OF "MONCTR"
;DATA FOR CENTER OPERATIONS
DATA
;RELATIVE POINTERS FOR "CTRYEL" AND "CTRBLU" DATA BLOCKS
RTTCHS ==0 ;SENSOR DATA TO SEND TO "TOUCH" SUBROUTINE
LFTCHS ==2
RTTEVT ==4 ;EVENT NUMBER FOR WAITING FOR RIGHT TOUCH SENSOR ACTIVE
LFTEVT ==6 ;SAME FOR LEFT
RTTPDB ==10 ;PTR TO PROCESS DESCRIPTOR BLOCK FOR CODE TO RUN ON RT. TCH.
LFTPDB ==12 ;SAME FOR LEFT
NUMTCH ==14 ;NUMBER OF TOUCH SENSORS ACTIVE: 0,1,2
CTRDVS ==16 ;PTR. TO DEVICE BLOCK FOR ARM PERFORMING "CENTER" FUNCTION
;DATA FOR OPERATING WITH YELLOW ARM
CTRYEL: 000402 ;TRIGGER IF YELLOW HAND RT SENSOR "ON"
000403 ; " " " " LF " "
0 ;EVENT NUMBER FOR RIGHT TOUCH SENSOR
0 ; " " " LEFT " "
YRTPDB ;PTR TO PDB FOR CODE TO RUN WHEN RT SENSOR ACTIVATED
YLFPDB ; " " " " " " " " LF " "
0 ;BOTH TOUCH SENSORS INITIALLY OFF
0 ;POINTER TO DEVICE BLOCK
;DATA FOR OPERATING WITH BLUE ARM
CTRBLU: 000400 ;TRIGGER IF BLUE HAND RT SENSOR "ON"
000401 ; " " " " LF " "
0 ;EVENT NUMBER FOR RIGHT TOUCH SENSOR
0 ; " " " LEFT " "
BRTPDB ;PTR TO PDB FOR CODE TO RUN WHEN RT SENSOR ACTIVATED
BLFPDB ; " " " " " " " " LF " "
0 ;BOTH TOUCH SENSORS INITIALLY OFF
0 ;POINTER TO DEVICE BLOCK
;RATES AT WHICH JOINTS AND HAND MOVE
RATE1: .FLT2 -.005 ;INITAL RATE OF HAND CLOSING
RATE2: .FLT2 -.0005 ;RATE OF HAND CLOSING AFTER TOUCH
JTRATE: .FLT2 .00025 ;DIFFERENTIAL CHANGE IN JT ANGLE/MSEC.
;ON MONITOR PROCESSOR DESCRIPTOR BLOCKS
YRTPDB: PDBLK 5,20,,CTRYEL
YLFPDB: PDBLK 5,20,,CTRYEL
BRTPDB: PDBLK 5,20,,CTRBLU
BLFPDB: PDBLK 5,20,,CTRBLU
CODE
;END OF "CENTER" DATA
;WHERE - UPDATES THE CURRENT JT ANGLE AND DYNAMIC COEF FOR SERVOS
;THE SERVOS SPECIFIED IN THE COEFFICIENT LIST HEADER ARE ATTACHED AND THEIR
;JOINT ANGLE, GRAVITY LOADING, AND JOINT INERTIA VALUES ARE UPDATED IN THEIR
;RESPECTIVE SERVO DATA BLOCKS. ONLY THE FIRST TWO WORDS OF THE COEFFICIENT
;LIST ARE USED BY THIS ROUTINE.
;
; COFLST: XXXXXX ;TWO SERVO BIT WORDS
; XXXXXX
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK
; JSR PC,WHERE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE BLOCK.
;THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS SUCCESS-
;FULLY ATTECHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE THIRD WORD OF
;THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE LOGICAL NUMBER OF
;ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN THAT SERVOS STATUS
;ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; AC0 IS GARBAGED
WHERE: MOV R0,WHRBLK ;SAVE POINTER TO COEFFICIENT DATA LIST
MOV #WHRBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE,SET UP IN DEVICE
; BLOCK
BCS 2$ ;EXIT IF ATTACH ERROR RETURNED
TST R0 ;BRANCH IF AT LEAST ONE SERVO ATTACHED
BGT 1$
MOV #WRGNUM,R0 ;SIGNAL ERROR
BR 2$ ;EXIT
;READ JOINT ANGLES AND UPDATE CURRENT JOINT INERTIA AND GRAVITY LOADING
1$: JSR PC,SETTH ;READ CURRENT JOINT ANGLES IN ALL ATTACHED SERVOS
BCS 2$ ;BRANCH IF WIPER ERROR OCCURRED
CLR R0 ;INDICATE NO PREPARATION ERROR
;EXIT CLEANLY, RELEASE ALL ATTACHED SERVOS
2$: JSR PC,RELSRV ;RELEASE SERVOS IN DEVICE BLOCK
RTS PC ;EXIT
;INFORMATION FOR SERVO DATA BLOCKS
DATA
WHRBLK: 0 ;POINTER TO COEF. DATA LIST
0 ;NOT A ARM MOTION FUNCTION
0
0
CODE
;END OF "WHERE"
;MOVE - MOVES THE ARM JOINTS ALONG A PRECOMPUTED TRAJECTORY
;THE JOINT SERVOS SPECIFIED IN THE COEFFICIENT LIST HEADER ARE DRIVEN
;ACCORDING TO PREDETERMINED POLYNOMIALS REPRESENTING THE JOINT ANGLES OR
;EXTENTIONS. ANY COMBINATION OF ARM JOINTS AND HAND JOINTS FROM THE
;"YELLOW" AND "BLUE" ARMS CAN BE SPECIFIED AS A SINGLE LOGICAL DEVICE.
;
;THE ORGANIZATION OF THE COEFFICIENTS DATA ARRAY IS EXACTLY AS PRESENTED ON
;PAGE 10 OF THIS PROGRAM. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 33 WRDS
; JSR PC,MOVE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. R1 IS LEFT POINTING AT THE DEVICE
;BLOCK. THE FIRST WORD OF THE DEVICE BLOCK WILL CONTAIN THE NUMBER OF SERVOS
;SUCCESSFULLY ATTACHED. THE SECOND WORD IS GARBAGED. STARTING WITH THE
;THIRD WORD OF THE DEVICE BLOCK, THE LOW BYTE OF THE WORD WILL CONTAIN THE
;LOGICAL NUMBER OF ONE OF THE SERVOS ATTACHED AND THE HIGH BYTE WILL CONTAIN
;THAT SERVOS STATUS ERROR BITS.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
.ifz short
.IFZ STDALN
MOVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
MOV R1,-(SP) ;SAVE POINTER TO DEVICE BLOCK
;SET UP DATA ARRAY TO BE TRANSFERRED
MOV @FPSAV,GPTR ;GET POINTERS FOR DATA GATHERING
MOV @ISAV,GIPTR
;ATTACH REQUIRED SERVOS
MOV #3,AWAIT
MOV R0,MVEBLK ;SAVE POINTER TO COEF. DATA LIST
MOV R0,R3
1$: MOV #MVEBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC 2$ ;BRANCH IF NO ATTACH ERRORS
SLEEP #1 ;SLEEP AND TRY AGAIN
DEC AWAIT
BGT 1$
JMP MVEDNE ;ERROR RETURNED, SIGNAL ATTACH ERROR AND EXIT
2$: TST R0 ;BRANCH IF AT LEAST ONE JOINT SELECTED
BGT 3$
MOV #WRGNUM,R0 ;ELSE SIGNAL ERROR
JMP MVEDNE ;EXIT CLEANLY
;SET UP JOINT WOBBLING IF NECESSARY
3$: BIT #WOBBLE,OPBITS(R3) ;WOBBLING?
BEQ 5$ ;NO
LDF @WOBMG(R3),AC0 ;HERE'S WERE WE GET THE WOBBLE MAGNITUDE
BIT #YARM,(R3) ;WOBBLING YELLOW ARM?
BEQ 4$ ;NO
STF AC0,YWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV04+WOBPTR
CLR SRV05+WOBPTR
MOV #20,SRV06+WOBPTR
4$: BIT #BARM,(R3) ;WOBBLING BLUE ARM?
BEQ 5$ ;NO
STF AC0,BWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV11+WOBPTR
CLR SRV12+WOBPTR
MOV #20,SRV13+WOBPTR
;INITIATE THE SERVOS FOR RUNNING
5$: MOV (SP),R1 ;POINT TO DEVICE BLOCK
MOV MVEBLK,R0 ;PTR TO COEF LIST
BIT #RUN,STFDAT ;IS STIFFNESS SERVO ENABLED?
BNE 7$
JSR PC,RUNSRV
BR 8$
7$: JSR PC,KMOVE
8$: TST GDATA ;WAS GATHER ENABLED?
BLE MVEDNE ;NO *k
MOV #30.,GCNT ;SAMPLE FOR 1/2 SECOND MORE
9$: JSR PC,REPS
JSR PC,RNS
SLEEP #16.
DEC GCNT
BGE 9$
; ASR GTIME ;COMPUTE NPTS for 30 hz sampling
MOV GIPTR,R1 ;GET INTEGER POINTER
MOV IDSAV,(R1)+ ;SAVE ID # *K
MOV GDATA,(R1)+ ;SAVE CONTROL BITS FOR GRAPH.SAI
MOV GTIME,(R1)+ ;SAVE NPTS FOR GRAPH.SAI
MOV R1,@ISAV ;REPLACE CHANGED POINTERS
MOV GPTR,@FPSAV
CLR GDATA
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
MVEDNE: MOV (SP)+,R1 ;RELEASE SERVOS IN DEVICE BLOCK
JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;LOCAL STORAGE AREA FOR "MOVE"
DATA
AWAIT: .WORD 3 ;WAIT TO ALLOW SERVOS TO BE RELEASED
;INFORMATION FOR SERVO DATA BLOCKS
MVEBLK: 0 ;POINTER TO COEF. DATA LIST
30000. ;ALLOW 30 SECONDS FOR FUNCTION
32. ;DELAY STARTING BY 32 MILLISECONDS
DATST ;POINTER TO START OF SEGMENT DATA
CODE
.ENDC
;MOVE - DIAGNOSTIC MOVE INSTRUCTION WITHOUT TRAJECTORY MODIFICATION
;THE ORGANIZATION OF THE COEFFICIENTS DATA ARRAY IS EXACTLY AS PRESENTED ON PAGE
;10 OF THIS PROGRAM. A PLANNED TRAJECTORY THAT IS INVALID DUE TO A CHANGE
;IN THE START OR END POINTS OF ANY OF ITS SEGMENTS IS CORRECTED BY THIS ROUTINE.
;TRAJECTORY MODIFICATION IS ACCOMPLISHED BY ADDING FIFTH ORDER POLYNOMIALS TO
;THE PLANNED POLYNOMIALS. CORRECTIONS ARE ALWAYS APPLIED BETWEEN VIA POINTS.
;IF TWO VIA POINTS HAVE ONE OR MORE FREE POINTS BETWEEN THEM, ALL OF THE SEGMENTS
;BETWEEN THE VIA POINTS ARE TREATED AS ONE SINGLE SEGMENT AND THE FIFTH ORDER
;CORRECTION POLYNOMIAL IS APPLIED ACROSS THIS SINGLE LONG SEGMENT.
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #COFLST,R0 ;SET POINTER TO COEFF. LIST
; MOV #DEVICE,R1 ;STORAGE AREA FOR DEVICE BLOCK, 33 WRDS
; JSR PC,MOVE
; TST R0 ;CHECK FOR ERROR CONDITION
;
;AT THE END OF EXECUTION, R0 CONTAINS AN ERROR CODE. THE POSSIBLE VALUES
;RETURNED IN R0 ARE LISTED ON PAGE 8. IN ADDITION, IF NO ERRORS OCCUR DURING
;PREPARATION, R1 RETURNS THE NUMBER OF SERVOS ATTACHED AND EACH WORD OF THE
;DEVICE BLOCK CONTAINS THE LOGICAL NUMBER OF ONE OF THE ATTACHED SERVOS IN
;ITS LOWER BYTE AND THAT SERVOS STATUS ERROR BITS IN THE UPPER BYTE.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE ALTERED
; AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
.IFNZ STDALN
MOVE: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
MOV R1,-(SP) ;SAVE POINTER TO DEVICE BLOCK
;ATTACH REQUIRED SERVOS AND DETERMINE NUMBER OF JOINTS AND ARMS TO BE RUN
MOV R0,MVEBLK ;SAVE POINTER TO COEF. DATA LIST
MOV R0,R3
MOV #MVEBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC 1$ ;BRANCH IF NO ATTACH ERRORS
JMP MVEDNE ;ERROR RETURNED, EXIT CLEANLY
1$: MOV R0,MVSRCT ;SAVE NUMBER OF ATTACHED SERVOS
;SET UP JOINT WOBBLING IF NECESSARY
BIT #WOBBLE,OPBITS(R3) ;WOBBLING?
BEQ 3$ ;NO
LDF @WOBMG(R3),AC0 ;HERE'S WERE WE GET THE WOBBLE MAGNITUDE
BIT #YARM,(R3) ;WOBBLING YELLOW ARM?
BEQ 2$ ;NO
STF AC0,YWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV04+WOBPTR
CLR SRV05+WOBPTR
MOV #20,SRV06+WOBPTR
2$: BIT #BARM,(R3) ;WOBBLING BLUE ARM?
BEQ 3$ ;NO
STF AC0,BWOBMG ;YES, SAVE WOBBLE DATA
CLR SRV11+WOBPTR
CLR SRV12+WOBPTR
MOV #20,SRV13+WOBPTR
;GET POINTER TO FIRST SEGMENT DATA AND DETERMINE IF THE STARTING POINT IS
;WITHIN JOINT ANGLE ERROR TOLERANCE
3$: MOV MVEBLK,R0 ;GET POINTER TO START OF COEF. DATA LIST
ADD #DATST+A0COEF,R0 ;ADDR OF 1ST A0 COEFFICIENT
MOV MVSRCT,R3 ;GET THE NUMBER OF JOINT SERVOS ATTACHED
ADD #4,R1 ;POINT TO THE FIRST SERVO DATA BLOCK
MVCMPT: MOV (R1)+,R4 ;GET ADDRESS OF SERVO BLOCK DATA
LDF TH(R4),AC0 ;LOAD THE CURRENT JOINT ANGLE
SUBF (R0),AC0 ;COMPUTE THE ERROR IN POSITION, TH-A0
STF AC0,AC1
ABSF AC0
CMPF ERRTOL(R4),AC0 ;COMPARE TO ERROR TOLERANCE
CFCC
BGE INTOL ;SKIP IF WITHIN TOLERANCE
MOV R0,R2 ;GET THE ADDR OF THE POLYNOMIAL TO MODIFY
MOV TH(R4),(R2)+ ;SET START POINT EQUAL TO CURRENT JT ANGLE
MOV TH+2(R4),(R2)+
NEGF AC1 ;HAVE TO CHANGE BY THIS MUCH TO GET BACK
MOV #A15TH,R4 ;GET ADDRESS OF CORRECTION POLYNOMIAL
MOV #5,R5 ;CORRECT NEXT 5 COEFFICIENTS
1$: LDF (R4)+,AC0 ;GET NORMALIZED COEFFICIENT
MULF AC1,AC0 ;COMPUTE TOTAL CHANGE
ADDF (R2),AC0 ;APPLY CORRECTION
STF AC0,(R2)+ ;AND SAVE
SOB R5,1$
INTOL: ADD #A5COEF-A0COEF+4,R0 ;POINT TO NEXT JOINT STARTING ANGLE
SOB R3,MVCMPT ;DO FOR ALL ATTACHED SERVOS
;INITIATE THE SERVOS FOR RUNNING
MOV (SP),R1 ;POINT TO DEVICE BLOCK
MOV MVEBLK,R0 ;PTR TO COEF. LIST
JSR PC,RUNSRV
;EXIT CLEANLY, RESTORE REGISTERS, RELEASE ALL ATTACHED SERVOS
MVEDNE: MOV (SP)+,R1 ;RELEASE SERVOS IN DEVICE BLOCK
JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC ;EXIT
;LOCAL STORAGE AREA FOR MOVE
DATA
MVEBLK: 0 ;POINTER TO COEF. DATA LIST
30000. ;ALLOW 30 SECONDS FOR FUNCTION
32. ;DELAY STARTING BY 32 MILLISECONDS
DATST ;POINTER TO START OF SEGMENT DATA
MVSRCT: 0 ;NUMBER OF ATTACHED JOINTS
CODE
.ENDC
.endc
;ATTSRV - ROUTINE FOR ATTACHING SERVOS
;"ATTSRV" FILLS THE DEVICE LIST WITH THE ADDRESSES OF THE SERVOS INDICATED
;BY THE SERVO BITS IN THE COEFFICIENT LIST HEADER. THE ADDRESS OF THE
;DEVICE LIST IS PLACED IN EACH SERVOS "INUSE" DATA BLOCK WORD TO INDICATE
;THAT THE SERVO IS ATTACHED TO A DEVICE. IF "INUSE" IS NOT INITIALLY ZERO
;IN ANY SERVO DATA BLOCK THAT IS TO BE ATTACHED, AN ERROR RETURN IS TAKEN.
;"DATPT", "POLY", "FCI", "TTIME", "COUNT" AND THE "MODBTS" WORD IN EACH SERVO
;DATA BLOCK ARE ALSO INITIALIZED. A SAMPLE CALL FOLLOWS:
;
; MOV #CBLK,R0 ;POINTER TO INFORMATION BLOCK
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,ATTSRV
; BCS ERROR ;CHECK FOR ERRORS
;WHERE
; CBLK: COEF ;POINTER TO COEFFICIENT LIST HEADER
; CNT ;MAXIMUM FUNCTION TIME FOR TIME OUT
; WAIT ;SERVO START WAIT TIME
; SEG ;REL. PTR. TO START OF SEGMENT DATA, 0 IF NO POLYNOMIALS
;
;THE DEVICE BLOCK IS LEFT IN THE FOLLOWING FORM:
;
; DEVICE: .BYTE NUM,0 ;WHERE NUM IS THE NUMBER OF SERVOS ATTACHED
; 0 ;LEAVES A BLANK TO PUT THE EVENT NUMBER IN
; SERVO-1 ;PTRS TO SERVO DATA BLOCKS
; SERVO-2
; :
; SERVO-NUM
;
;AFTER EXECUTION, IF AN ATTACH ERROR OCCURRED, THE C BIT IS SET AND R0 CONTAINS
;THE ERROR CODE "CNTATT", OTHERWISE, R0 CONTAINS THE NUMBER OF DEVICES ATTACHED.
;DEFINITIONS:
SETCNT==2
SETSWT==4
RELSEG==6
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
ATTSRV: MOV R5,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
MOV R3,-(SP)
MOV R2,-(SP)
MOV R1,-(SP) ;SAVE DEVICE BLOCK ADDRESS
MOV (R0),R4 ;GET ADDRESS OF COEF. LIST
MOV (R4),R2 ;GET THE SERVO BITS TO DETERMINE # OF REQ. SRV.
MOV 2(R4),R3 ;32 POSSIBLE SERVOS
CLR R5
TST R2 ;CHECK IF SERVO #1 REQUESTED
1$: BPL .+4 ;SKIP IF THIS SERVO NOT REQUESTED
INC R5 ;ELSE INCREMENT SERVO COUNT
ASHC #1,R2 ;GET NEXT SERVO BIT
BNE 1$ ;REPEAT TILL ALL SERVO BITS ADDED
MUL #A5COEF-A0COEF+4,R5 ;COMPUTE OFFSET TO START OF DYNAMIC COEF.
MOV R5,SETFCI ;SAVE OFFSET TO CII AND CI COEFFICIENTS
MOV OPBITS(R4),SETMOD ;GET THE COMMAND MODE BITS
MOV #A5COEF,SETPLY ;GET INITIAL REL PTR TO POLY DATA
MOV RELSEG(R0),SETSEG ;GET REL. PTR TO START OF SEGMENT DATA
BEQ 2$ ;BRANCH IF NO POLYNOMIAL TO FOLLOW
ADD (R0),SETSEG ;CONVERT SEG PTR TO ABSOLUTE ADDRESS
ADD SETSEG,SETPLY ;CONVERT POLY PTR TO ABSOLUTE ADDRESS
ADD SETSEG,SETFCI ;CONVERT DYNA. COEF. PTR TO ABS. ADDR.
ADD #A0COEF,SETFCI
2$: ADD #4,R1 ;START STORING SERVO PTRS. IN HERE
MOV (R4),R2 ;GET THE SERVO BITS AGAIN
MOV 2(R4),R3 ;32 POSSIBLE SERVOS
MOV #SERVOS,R4 ;ADDR. OF TABLE CONTAINING SERVO DATA BLK PTRS
MOV (R4)+,R5 ;GET THE POINTER TO THE SERVO DATA BLOCK
CLR @(SP) ;ZERO SERVO COUNTER
TST R2
ATTLP: BPL NXTSRV ;BRANCH IF THIS SERVO IS NOT TO BE USED
TST INUSE(R5) ;CHECK IF SERVO ATTACHED TO ANOTHER DEVICE
BEQ NOTIUS ;BRANCH IF NOT ALREADY ATTACHED
ATTERR: MOV #CNTATT,R0 ;SHOULDN'T BE ATTACHED, SIGNAL ERROR
SEC
BR ATTDNE
NOTIUS: TST (R5) ;CHECK STATUS WORD OF DEVICE
BNE ATTERR ;ALL STATUS BITS SHOULD BE OFF
MOV (SP),INUSE(R5) ;PUT DEVICE ADDR. IN SERVO BLOCK TO INDICATE
; SERVO IS ATTACHED
; BITB #VIS+SCRE,MECHSM(R5) ;SKIP SETTING IF VSE OR SCRE *K
; BNE 1$
MOV SETCNT(R0),COUNT(R5) ;SET COUNT FOR MAXIMUM FUNCTION TIME
MOV SETSWT(R0),TTIME(R5) ;SET START WAIT TIME
MOV SETSEG,DATPT(R5) ;SET POINTER TO SEGMENT DATA
MOV SETMOD,MODBTS(R5) ;SET THE OPERATION MODE BITS
MOV SETPLY,POLY(R5) ;SET PTR TO POLYNOMIAL COEF
MOV SETFCI,FCI(R5) ;SET PTR TO DYNAMIC COEF.
ADD #24.,SETPLY ;INCREMENT PTRS TO NEXT BLOCK OF COEF
ADD #10,SETFCI
1$: MOV R5,(R1)+ ;PUT SERVO DATA ADDR. IN DEVICE LIST
INCB @(SP) ;ADD 1 TO THE SERVO COUNT
NXTSRV: MOV (R4)+,R5 ;POINT TO NEXT SERVO DATA BLOCK
ASHC #1,R2 ;GET NEXT SERVO BIT
BNE ATTLP ;REPEAT IF SOME SERVO BITS LEFT
MOV @(SP),R0 ;RETURN NUMBER OF SERVOS ATTACHED
CLC ;INDICATE NO ATTACH ERRORS
ATTDNE: MOV (SP)+,R1 ;RESTORE REGISTERS
MOV (SP)+,R2
MOV (SP)+,R3
MOV (SP)+,R4
MOV (SP)+,R5
RTS PC ;NORMAL RETURN
;LOCAL STORAGE AREA FOR "ATTSRV"
DATA
SETMOD: 0 ;OPERATION MODE BITS TO BE PUT IN MODBTS WORD
SETPLY: 0 ;POINTER TO START OF POLYNOMIAL COEF.
SETFCI: 0 ;POINTER TO DYNAMIC COEFFICIENTS, CII & CI
SETSEG: 0 ;POINTER TO SEGMENT DATA
CODE
;END OF "ATTSRV"
;RELSRV - ROUTINE FOR DETACHING SERVOS
;"RELSRV" RELEASES ALL SERVOS POINTED TO BY THE DEVICE BLOCK BY ZEROING
;INUSE WORDS AND STATUS WORD RUN ERROR FLAGS IN THE SERVO DATA BLOCKS. THE
;POINTERS TO THE ATTACHED SERVOS IN THE DEVICE BLOCK ARE REPLACED BY THE
;LOGICAL NUMBER OF THE SERVO AND ITS ASSOCIATED RUN ERROR BITS. THE SERVO
;NUMBER IS PLACED IN THE LOWER BYTE AND THE ERROR BITS IN THE UPPER BYTE OF
;THE DEVICE BLOCK WORD. ALL OF THE SERVO RUN ERROR BITS ARE OR'ED WITH
;REGISTER R0. A SAMPLE CALL FOLLOWS:
;
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,RELSRV
; TST R0 ;CHECK FOR RUN ERROR
;
;AFTER EXECUTION, THE FIRST WORD OF THE DEVICE BLOCK CONTAINS THE NUMBER
;OF SERVOS INITIALLY ATTACHED.
;REGISTERS USED:
; R0, R1 PASS ARGUMENTS AND R0 IS ALTERED
RELSRV: MOV R3,-(SP) ;SAVE REGISTERS
MOV R2,-(SP)
MOV R1,-(SP)
MOV R0,-(SP)
MOVB (R1),R3 ;GET THE NUMBER OF SERVOS ATTACHED
MOV R3,@2(SP) ;LEAVE NUMBER IN FIRST WORD OF DEVICE BLK
ADD #4,R1 ;SERVO POINTERS START HERE
TST R3 ;CHECK IF ANY SERVOS LEFT TO DETACH
1$: BLE 2$ ;BRANCH IF END OF DEVICE LIST
MOV (R1),R2 ;GET THE ADDR. OF A SERVO DATA BLOCK
BIS (R2),R0 ;OR ALL RUN ERROR BITS TOGETHER
MOVB SRVNUM(R2),(R1)+ ;SEND BACK SERVO NUMBER
MOVB 1(R2),(R1)+ ;SEND BACK RUN ERROR BITS
JSR PC,SETSET ;RESET ALL JOINT VARIABLES
DEC R3 ;DECREMENT NUMBER OF SERVOS LEFT TO DETACH
BR 1$ ;GO CHECK IF MORE SERVOS TO DO
2$: BIC #377,R0 ;ISOLATE SERVO RUN ERROR BITS
BIS (SP)+,R0 ;OR ON PERVIOUS ERROR BITS
MOV (SP)+,R1 ;RESTORE DEVICE BLOCK POINTER
MOV (SP)+,R2
MOV (SP)+,R3
RTS PC ;RETURN
;END OF "RELSRV"
;SETSET - RESETS SERVO BLOCK VARIABLES AFTER A MOVE IS COMPLETED
SETSET: CLRF THD(R2) ;SET ALL VELOCITIES TO ZERO
CLRF THDP(R2)
MOV TH(R2),THP(R2) ;SET PREDICATED JOINT ANGLE TO ACTUAL
MOV TH+2(R2),THP+2(R2)
CLRF DELTH(R2) ;NO DEVIATION FROM TRAJECTORY AT END
CLRF DELTHD(R2) ;NO FINAL CONSTANT VELOCITY
CLRF ERRINT(R2) ;ZERO INTEGRAL OF POSITION ERROR
CLRF POSERR(R2) ;NO FINAL POSITION ERROR
CLRF ERRTQE(R2) ;ZERO ERROR TORQUE
CLRF YOLD(R2) ;ZERO FORCE FILTER TERM
CLRF UOLD(R2) ;ZERO FORCE FILTER TERM
CLR INUSE(R2) ;CLEAR IN USE FLAG, DETACH SERVO
CLRB DACVAL(R2) ;ZERO DAC OUTPUT
BIC #-1#NONEX,(R2) ;CLEAR STATUS WORD ERROR BITS
BIC #NNUL+DEPTPT+WOBBLE,MODBTS(R2) ;RESET MODE WORD
RTS PC
;END OF "SETSET"
;SETTH - READS A/D AND INITIALIZES JOINT ANGLES FOR A GIVEN DEVICE BLOCK
;LEVEL 7 CODE IS INITIATED TO READ THE REQUIRED JOINT ANGLES FROM THE ADC.
;THE LEVEL 7 CODE IS REPEATED UNTIL ALL OF THE WIPERS ARE WITH IN OPERATING
;RANGE OR UNTIL 4 ATTEMPTS HAVE BEEN MADE, WHICHEVER OCCURS FIRST. IF AT
;THE END SOME WIPERS ARE STILL OUT OF RANGE, AN ERROR RETURN IS TAKEN. "TH",
;THE ACTUAL JOINT ANGLE, AND "THP", THE PREDICTED JOINT ANGLE ARE INITIALIZED
;TO THE SAME VALUE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,SETTH
; BCS ERROR ;BRANCH IF ERROR DURING SETTH
;
;IF A WIPER ERROR OCCURS, THE C BIT IS SET AND R0 CONTAINS THE ERROR CODE
;"BADWIP" WHEN THIS ROUTINE EXITS. IF NO WIPER ERROR OCCURRED, NEW DYNAMIC
;COEFFICIENTS ARE COMPUTED FOR ANY ARM THAT HAD ANY OF ITS JOINT ANGLES
;UPDATED.
;REGISTERS USED:
; R1 PASSES ARGUMENTS AND IS NOT MODIFIED
; R0, AC0 ARE GARBAGED
SETTH: MOV R1,-(SP) ;SAVE REGISTERS
MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
1$: INC STTLCK ;CHECK IF LOCKED OUT
BEQ 2$ ;BRANCH IF FREE TO GO ON
DEC STTLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR 1$
2$: MOV #STTSRV,R2 ;PUT LIST OF SERVOS WITH POTS TO READ IN HERE
MOVB (R1),R4 ;GET NUMBER OF SERVOS ATTACHED
BEQ STTDNE ;EXIT CLEANLY IF THERE ARE NONE
ADD #4,R1 ;SERVO POINTERS START HERE
CHKPOT: MOV (R1)+,R0 ;GET POINTER TO SERVO DATA BLOCK
TST POTCHN(R0) ;CHECK IF SERVO HAS A POT TO READ
BMI 1$ ;SKIP IF NO POT
MOV R0,(R2)+ ;SAVE SERVO POINTER
1$: SOB R4,CHKPOT ;GO CHECK NEXT POT
CLR (R2) ;INDICATE END OF SERVO LIST
TST STTSRV ;CHECK IF ANY POTS TO READ
BEQ STTDNE ;EXIT CLEANLY IF THERE ARE NONE
MOV #4,STTCNT ;EXECUTE LEVEL7 CODE AT MOST 4 TIMES
.IFZ SNGSTP
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),STTEVT ;SAVE THE EVENT NUMBER
SCHED7 #STTPPB,#1
EVWAIT (SP) ;WAIT FOR THE LEVEL 7 JOB TO FINISH
EVKIL
.IFF
JSR PC,STHLV7 ;EXECUTE LEVEL SEVEN CODE
.ENDC
TST STTWIP ;CHECK IF ALL WIPERS IN RANGE
BEQ 2$ ;BRANCH IF WIPERS OK
SEC ;INDICATE ERROR
MOV #BADWIP,R0
BR STTERR ;EXIT CLEANLY
;CONVERT THE A/D READINGS TO JOINT ANGLES FOR ALL SERVOS WITH POTS
2$: MOV #STTSRV,R4 ;SET POINTER TO LIST OF SERVOS WITH POTS
CLRB -(SP) ;COLLECT SERVO MECHANISM BITS IN HERE
STTLP: MOV (R4)+,R5 ;GET POINTER TO SERVO BLOCK DATA
BEQ COMPCI ;BRANCH IF END OF LIST
BISB MECHSM(R5),(SP) ;OR ALL MECHANISM BITS TOGETHER
MOV POT(R5),R0 ;GET THE POT READING
ADD #4000,R0 ;SHIFT POT READING TO 0→7777
;ADD CORRECTION FOR NON-LINEAR POT ELEMENT AND CONVERT TO FLOATING POINT
.IFZ ISLIN
MOV R0,R3 ;SAVE NORMALIZED POT READING
CLR R1
ASHC #-10,R0 ;GET 4 MSB TO USE AS INDEX INTO NON-LINEAR TABLE
ROR R1 ;LEAVE 8 LSB AS POSITIVE TWOS COMPLEMENT NUMBER
ADD R5,R0 ;COMPUTE A POINTER INTO THE NON-LINEAR CORR. TABLE
MOVB NONLIN(R0),R2 ;GET LOWER LIMIT OF CALIBRATION TABLE
MOVB NONLIN+1(R0),R0 ;GET UPPER LIMIT TO INTERPOLATE
SUB R2,R0 ;GET DIFFERENCE
MUL R1,R0 ;INTERPOLATE
ASHC #1,R0 ;GET 16 MSB
ADD R2,R0 ;NOW HAVE PROPER CALIBRATION
ADD R3,R0 ;ADD TO NORMALIZED POT READING
.ENDC
LDCIF R0,AC0 ;CONVERT TO FLOATING POINT
MULF SCALE(R5),AC0 ;CONVERT FROM A/D UNITS TO JOINT ANGLE
ADDF OFFSET(R5),AC0 ;ADD POT OFFSET
STF AC0,TH(R5) ;SAVE THE JOINT ANGLE WITH THE SERVO DATA
STF AC0,THP(R5) ;SAVE AS PREDICTED JOINT ANGLE
BR STTLP ;GO DO NEXT SERVO
;RECOMPUTE DYNAMIC COEFFICIENTS IF ANY ARM JOINT ANGLES READ
COMPCI: MOVB (SP)+,R3 ;GET MECHANISM BITS FOR ALL SERVOS UPDATED
;PERHAPS NEXT INSTRUCITON SHOULD BE MOV #YELARM,R2 TO SAY YELLOW ARM TO DTERMS...*K1
CLR R2 ;CHECK IF YELLOW ARM JOINT ANGLES UPDATED
BIT #YELARM,R3
BEQ 1$ ;BRANCH IF NOT
MOV #YTH,R0 ;ELSE COMPUTE DYNAMIC COEF, BASED UPON JT. ANGS.
MOV #YNCI,R1 ;SAVE IN SERVO DATA BLOCKS
JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
1$: BIT #BLUARM,R3 ;CHECK IF BLUE ARM JOINT ANGLES UPDATED
BEQ STTDNE ;BRANCH IF NOT
MOV #BTH,R0 ;ELSE COMPUTE CURRENT DYNAMIC COEF
MOV #BNCI,R1 ;UPDATE SERVO DATA BLOCKS
INC R2 ;INDICATE BLUE ARM *K MOV #BLUARM,R2
JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
;EXIT CLEANLY
STTDNE: CLC ;INDICATE NO ERROR
STTERR: DEC STTLCK ;RELEASE CODE FOR FUTURE USE
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
MOV (SP)+,R1
RTS PC
;LOCAL STORAGE AREA
DATA
STTLCK: -1 ;-1 → SETTH READY FOR USE, 0 → LOCKED UP
STTEVT: .BLKW 1 ;LEVEL 7 CODE COMPLETION EVENT
STTCNT: 4 ;NUMBER OF PASSES PERMITTED THROUGH LEVEL 7 CODE
STTWIP: 0 ;WIPER ERROR CODE CORRESPONDING TO "OUTRGE"
STTSRV: .BLKW 37. ;LIST OF SERVOS WITH POTS
STTPPB: STTPDB ;ARRAY INITIALIZE LEVEL 7 CODE
.WORD 0,0 ;TIME AND END OF LIST
STTPDB: PDBLK7 STHLV7,STTSRV,20
CODE
;END OF "SETTH"
;SETREF - READ A/D, SET REFERENCE VOLTAGE AND ZERO TACH VALUE READINGS
;LEVEL 7 CODE IS INITIATED TO READ THE REFERENCE VOLTAGE SUPPLY AND ANY
;TACHOMETER CHANNELS THAT MAY EXIST FOR THE SERVOS TO BE USED. A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #DEVICE,R1 ;POINTER TO DEVICE BLOCK
; JSR PC,SETREF
;
;THE REFERENCE VOLTAGE READING AND THE TACHOMETER ZERO READINGS ARE READ
;AND AVERAGED FOUR TIMES TO REDUCE THE NOISE LEVEL. IF ANY OF THE READINGS
;DEVIATES SIGNIFICANTLY FROM THE PREVIOUS AVERAGE, AN ERROR CODE IS RETURNED
;IN R0, OTHERWISE R0 IS SET TO ZERO.
;REGISTERS USED:
; R1 PASSES ARGUMENT AND IS UNALTERED
; R0 RETURNS AN ERROR CODE
SETREF: MOV R1,-(SP) ;SAVE REGISTERS AND PTR TO DEVICE BLOCK
MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
1$: INC SRFLCK ;CHECK IF LOCKED OUT
BEQ 2$ ;BRANCH IF FREE TO GO ON
DEC SRFLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR 1$
2$: MOV #SRFSRV+4,R2 ;SET POINTER TO LIST OF SERVOS WITH TACHS,SKIP
; OVER POWER SUPPLY REFERENCE READINGS
MOV #SRFADL+4,R3 ;SET POINTER TO A/D LIST, SKIP OVER REF. CHANNELS
;*K WE NEED MORE REF CHANNEL SPACE FOR YELLOW INTERFACE
MOVB (R1),R4 ;GET NUMBER OF SERVOS ATTACHED
BEQ ALLTAC ;BRANCH IF NO SERVOS ATTACHED
ADD #4,R1 ;POINT TO FIRST SERVO DATA BLOCK PTR IN DEVICE BLK
CHKTAC: MOV (R1)+,R0 ;GET PTR TO SERVO DATA BLOCK
TST TACCHN(R0) ;CHECK IF SERVO HAS A TACHOMETER
BMI 1$ ;SKIP IF NO TACH
MOV R0,(R2)+ ;SAVE SERVO POINTER
MOV TACCHN(R0),(R3)+ ;SAVE POT CHANNEL IN A/D LIST
MOV TACHZ0(R0),TACH(R0) ;SAVE PREVIOUS TACH ZERO READING
1$: SOB R4,CHKTAC ;REPEAT IF ANY SERVOS LEFT
ALLTAC: MOV #-1,(R3) ;INDICATE END OF A/D LIST
CLR (R2) ;INDICATE END OF SERVO LIST
MOV #4,SRFCNT ;AVERAGE ADC READINGS FOUR TIMES
.IFZ SNGSTP
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),SRFEVT ;SAVE EVENT NUMBER FOR LEVEL 7 CODE
SCHED7 #SRFPPB,#1
EVWAIT (SP) ;WAIT FOR THE LEVEL 7 JOB TO FINISH
EVKIL
.IFF
JSR PC,SRFLV7 ;EXECUTE LEVEL 7 CODE IN DIAGNOSTIC MODE
.ENDC
;SECTION TO CHECK IF READINGS WITHIN TOLERANCE, START WITH REFERENCE
MOV REFVT1,R0 ;GET NEW REFERENCE VOLTAGE READINGS
SUB REFER1,R0 ;GET DIFFERENCE FROM CALIBRATION VALUE
BGT .+4 ;TAKE ABSOLUTE VALUE
NEG R0
CMP #REFTOL,R0 ;COMPARE TO ERROR TOLERANCE
BLE 1$ ;BRANCH IF REFERENCE OUT OF RANGE
MOV REFVT2,R0 ;REPEAT FOR SECOND REFERENCE READING
SUB REFER2,R0 ;GET DIFFERENCE FROM CALIBRATION VALUE
BGT .+4 ;TAKE ABSOLUTE VALUE
NEG R0
CMP #REFTOL,R0 ;COMPARE TO ERROR TOLERANCE
BGT 2$ ;BRANCH IF REFERENCE IN OF RANGE
1$: MOV #BADREF,R0 ;ELSE SIGNAL ERROR
SEC
BR SEFDNE ;EXIT
2$: MOV #SRFSRV+4,R3 ;GET POINTERS TO SERVOS WITH TACHS
3$: MOV (R3)+,R4 ;GET SERVO DATA BLOCK ADDRESS
BEQ TACSOK ;BRANCH IF END OF LIST
MOV TACHZ0(R4),R0 ;GET TACH. ZERO VELOCITY READING
SUB TACH(R4),R0 ;COMPUTE DIFF. FROM PREVIOUS VALUE
BPL .+4 ;TAKE ABSOLUTE VALUE
NEG R0
CMP #10,R0 ;CHECK IF WITH LIMITS
BGE 3$ ;GO CHECK NEXT ONE IF OK
MOV #BADTAC,R0 ;ELSE SIGNAL ERROR
SEC
BR .+4 ;EXIT
TACSOK: CLC ;SIGNAL NO ERROR
;SECTION TO EXIT FROM "SETREF"
SEFDNE: DEC SRFLCK ;RELEASE CODE FOR FUTURE USE
MOV (SP)+,R4 ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R2
MOV (SP)+,R1
RTS PC ;RETURN
;LOCAL STORAGE AREA
DATA
SRFLCK: -1 ;-1 → SETREF READY FOR USE, 0 → LOCKED UP
SRFCNT: .BLKW 1 ;EXECUTION COUNTER FOR LEVEL 7 CODE
SRFEVT: .BLKW 1 ;LEVEL 7 CODE COMPLETION EVENT
SRFSRV: REFVT1-TACHZ0 ;STORAGE LOCATION FOR REF. POWER SUPPLY READINGS
REFVT2-TACHZ0
.BLKW 37. ;LIST OF SERVOS WITH TACHOMETERS
SRFADL: REFCN1 ;REFERENCE VOLTAGE SUPPLY A/D CHANNELS
REFCN2
.BLKW 37. ;A/D CHANNEL LIST
SRFVAL: .BLKW 38. ;A/D READINGS
SRFPPB: SRFPDB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0 ;TIME AND END OF LIST MARKER
SRFPDB: PDBLK7 SRFLV7,SRFSRV,20
CODE
;END OF "SETREF"
;STHLV7 & SRFLV7 - LEVEL 7 CODE FOR "SETTH" AND "SETREF"
;STHLV7 IS USED BY "SETTH" TO READ THE ADC POT CHANNELS. THIS ROUTINE IS
;REPEATED UNTIL ALL POTS ARE READ WITHIN WIPER RANGE OR UNTIL "STTCNT" IS
;DECREMENTED TO ZERO. "SETTH" MUST BE SCHEDULED TO RUN AT LEVEL 7.
;IT IS ASSUMED THAT THE KERNEL PRELOADS REGISTER "DAT" WITH A POINTER
;TO A TABLE OF POINTERS TO SERVOS WITH POTS TO BE READ.
STHLV7:
.IFNZ SNGSTP
MOV #STTSRV,DAT ;SET POINTER TO SERVOS WITH POTS
.ENDC
MOV #42,DR11S ;SET YELLOW INTERFACE TO READ MODE
CLR OUTRGE ;ASSUME ALL WIPERS IN RANGE
MOV DAT,BC ;SAVE POINTER TO SERVOS WITH POTS
MOV (BC)+,DAT ;SET POINTER TO FIRST SERVO DATA BLOCK
MOVB POTCHN(DAT),ADCCHN ;START ADC WORKING ON FIRST CHANNEL
MOVB POTCHN(DAT),DR11O ;IN BOTH INTERFACES
1$: BITB #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE
BEQ 4$
TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV ADCVAL,POT(DAT) ;SAVE ADC POT READING
BR 5$
4$: TSTB DR11S
BPL .-4 ;WAIT
MOV DR11I,POT(DAT) ;SAVE ADC POT READING
5$: MOV (BC)+,CC ;GET POINTER TO NEXT SERVO DATA BLOCK
BEQ 7$ ;SKIP IF END OF LIST
BITB #BLUARM+BLUHND,MECHSM(CC)
BEQ 6$
MOVB POTCHN(CC),ADCCHN ;START CONVERTING NEXT CHANNEL
BR 7$ ;*K1
6$: MOVB POTCHN(CC),DR11O
7$: JSR PC,WIPER ;CHECK IF WIPER WITHIN OPERATING RANGE
MOV CC,DAT ;REPEAT TILL ALL SERVOS DONE
BNE 1$
DEC STTCNT ;HAS THIS CODE RUN 4 TIMES ALREADY?
BLE 2$ ;BRANCH IF NO MORE CHANCES TO GET WIPERS IN RANGE
TST OUTRGE ;ELSE TEST IF ANY WIPERS OUT OF RANGE
BEQ 2$ ;WE ARE DONE IF NO WIPERS OUT OF RANGE
.IFZ SNGSTP
RESCH7 #16. ;RE-SCHEDULE THIS CODE TO RUN AGAIN
TST (SP)+ ;CLEAR SCHEDULED TIME OFF STACK
DISMS7 ;GO TO SLEEP
.ENDC
2$: MOV OUTRGE,STTWIP ;SAVE WIPER ERROR CODE
.IFZ SNGSTP
EVSIG STTEVT ;SIGNAL END OF LEVEL 7 CODE
DISMS7 ;GO KILL THE JOB
.IFF
RTS PC ;RETURN IF IN DIAGNOSTIC MODE
.ENDC
;SRFLV7 IS CALLED BY "SETREF" TO READ THE ADC TACHOMETER CHANNELS AND THE
;REFERENCE POWER SUPPLY VOLTAGE. THIS ROUTINE RESCHEDULES ITSELF TO AVERAGE
;THE READINGS "SRFCNT" NUMBER OF TIMES.
SRFLV7:
.IFNZ SNGSTP
MOV #SRFSRV,DAT ;SET POINTER TO SERVOS WITH TACHOMETERS
.ENDC
MOV #SRFADL,DC
MOV #SRFVAL,EC ;SET POINTER TO A/D READINGS
MOV DAT,CC
1$: MOV (CC)+,DAT ;SET POINTER TO SERVO BLOCK DATA
BEQ 2$ ;BRANCH IF END OF LIST
MOV (DC)+,AC
cmp cc,#srfsrv+6 ;are we checking blue ref channels?
blt 6$ ;br if yes
; cmp cc,#srfsrv+12 ;are we checking yellow ref channels?
; blt 4$ ;br if yes
BITB #BLUARM+BLUHND,MECHSM(DAT) ;SELECT INTERFACE FOR THIS SERVO
BEQ 4$
6$: MOVB AC,ADCCHN ;BLUE INTERFACE
TSTB ADCSR
BPL .-4 ;WAIT
MOV ADCVAL,AC ;GET TACH READING
BR 5$
4$: MOV #42,DR11S ;YELLOW INTERFACE *K
MOVB AC,DR11O
TSTB DR11S
BPL .-4 ;WAIT
MOV DR11I,AC ;GET TACH READING
SUB #3777,AC
5$: SUB TACHZ0(DAT),AC
ASR AC ;GET A 1/2 OF THE DIFFERENCE
ADD AC,TACHZ0(DAT) ;USE THIS TO CORRECT THE OLD READING
BR 1$ ;DO ALL SERVOS WITH TACHS
2$: DEC SRFCNT ;HAVE WE FINISHED AVERAGING YET?
BLE 3$ ;BRANCH IF WE ARE ALL THROUGH
.IFZ SNGSTP
RESCH7 #16. ;RE-SCHEDULE THIS CODE TO RUN AGAIN
TST (SP)+ ;CLEAR SCHEDULED TIME OFF STACK
DISMS7 ;GO TO SLEEP
.ENDC
3$:
.IFZ SNGSTP
EVSIG SRFEVT ;SIGNAL END OF LEVEL 7 CODE
DISMS7 ;GO KILL THE JOB
.IFF
RTS PC ;RETURN IF IN DIAGNOSTIC MODE
.ENDC
;RUNSRV - SCHEDULES THE SERVOS OF A DEVICE TO RUN AND WAITS FOR THEIR COMPLETION
;THE FOLLOWING FUNCTIONS ARE PERFORMED BY THIS ROUTINE:
;
; 1) THE HARDWARE ERROR RESET IS TOGGLED AND A CHECK IS MADE TO
; ENSURE THAT THE POWER SUPPLY IS ON. IF IT'S NOT, AN ERROR FLAG
; IS RETURNED IN R0 OTHERWISE R0 IS CLEARED.
; 2) THE SERVOS LISTED IN THE DEVICE BLOCK ARE SCHEDULED FOR EXECU-
; TION AT LEVEL 7 ALONG WITH A DAC JOB TO REFRESH THE DACS.
; 3) THE TIME THAT EACH SERVO IS SCHEDULED TO FIRST RUN IS SAVED IN
; "PTIME" WITHIN EACH SERVO DATA BLOCK.
; 4) THE RUN BIT IS SET IN EACH SERVO STATUS WORD.
; 5) IF EITHER FORCE SENSING OR COMPLIANCE IS TO BE PERFORMED WITH
; THE ARM SPECIFIED BY THE SERVOS, THE FORCE SENSING LEVEL 7&5
; JOBS ARE STARTED AND SYNCHRONIZED TO THE ARM JOINTS. AN ERROR
; MESSAGE IS RETURNED AND THE FORCE SENSING IS ABORTED IF NOT
; ALL 6 OF THE ARM JOINTS ARE SCHEDULED TO RUN.
; 6) AFTER ALL SERVOS ARE COMPLETED, THIS ROUTINE KILLS THE DAC
; REFRESH JOB AND THE FORCE SENSING JOBS AND RETURNS TO THE
; CALLING ROUTINE.
;
;THIS SUBR IS CALLED WITH A PTR TO THE COEFFICIENT DATA LIST IN R0 AND
;A PTR TO THE DEVICE BLOCK IN R1
;REGISTERS USED:
; R1,R0 PASS ARGUMENT AND R0 IS ALTERED
RUNSRV: MOV R4,-(SP)
MOV R3,-(SP)
MOV R2,-(SP)
MOV (R1),-(SP) ;SAVE NUMBER OF SERVOS ATTACHED
MOV R1,-(SP) ;SAVE DEVICE BLOCK POINTER
MOV (R0),R3 ;GET SERVO BITS
.IFNZ TIMER
MOV #TIMES+2,TIMES ;RESET POINTER TO SAVE EXECUTION TIME DATA
.ENDC
.IFNZ FRCDAT
MOV #FDATA+2,FDATA ;RESET PTR TO FORCE DATA ARRAY
.ENDC
clr r4 ;assume no force sensing needed
tst gdata ;are we gathering?
bne 1$ ;YES
tst frcnum ;any jobs waiting on force sensing?
beq refres ;NO
1$: BIT #YARM+BARM,R3 ;ARM BEING USED?
BEQ REFRES ;NO
MOV #BARM,R0 ;YES, ASSUME BLUE ARM
MOV #SRV13,R2
BIT #BLUARM,@FCARAY ;WERE WE RIGHT?
BNE .+12
MOV #YARM,R0 ;NO
MOV #SRV06,R2
BIC R3,R0 ;MAKE SURE ALL 6 JTS ASSIGNED
BEQ 2$ ;OK
MOV #FWRGJT,R0 ;SIGNAL ERROR
JMP RUNDNE
2$: MOV DATPT(R2),FDATPT;SAVE POLYNOMIAL DATA FOR FORCE RTNS
BNE 3$
MOV #NOPOLY,R0 ;SIGNAL CAN'T FORCE SERVO WITHOUT POLY
BR RUNDNE
3$: MOV POLY(R2),FPOLY
ADD #4,FPOLY
MOV INUSE(R2),FINUSE
MOV TTIME(R2),FTTME
MOV #FRCPBB,R4 ;SCHEDULE FORCE LEVEL 7 TO RUN
REFRES: BIT #BARM+BHAND,R3 ;BLUE ARM?
BEQ 1$ ;NO
INCB KICDAC ;NEED TO SCHEDULE DAC REFRESH JOB?
BIT #KICRUN,KICDAC
BNE 1$ ;NO, STILL RUNNING
MOVB #10,174004 ;RESET BLUE ARM INTERFACE
MOV #NOPOWR,R0
BIT #6,174004 ;MOTOR POWER ON?
BNE DECDAC ;NO, SIGNAL ERROR
CLR BPANIC ;CLEAR BLUE ARM HARDWARE ERROR FLAG
SCHEDU #MUGDAC,#DACSER,#USRDM,#20
BIS #KICRUN,KICDAC
1$: MOV #41,DR11S ;ENABLE YELLOW INTERUPT REQ B *K
MOV #104000,DR11O ;ENABLE YELLOW PANIC INTERUPT AND DACS
EVMAK ;THIS EVENT SIGNALS SERVOS DONE
MOVB (R1),R0 ;GET THE NUMBER OF SERVOS TO SCHEDULE
TST (R1)+
MOV (SP),(R1)+ ;SAVE EVENT NUMBER IN DEVICE BLOCK
MOV #SCHBLK,R2 ;CREATE ARRAY OF PDB'S TO PASS TO MONITOR
2$: MOV (R1)+,(R2) ;GET A SERVO DATA BLOCK PTR
ADD #SPDBLK,(R2)+ ;POINT TO PDB OF SERVO
TST (R2)+ ;ALLOW ROOM FOR THE MON. TO RETURN THE SCH.TIME
SOB R0,2$ ;REPEAT FOR ALL ATTACHED SERVOS
MOV R4,(R2)+ ;THIS EITHER PUTS THE FORCE LEVEL 7 JOB IN
TST (R2)+ ; THE SCHEDULE LIST OR MARKS THE END
CLR (R2)
.IFZ SNGSTP
SCHED7 #SCHBLK,SCTIME ;SCHEDULE ALL LEVEL7 JOBS
.ENDC
MOV #SCHBLK,R2 ;SAVE TIMES THAT THE SERVOS WHERE SCH. FOR
MOVB 4(SP),R0 ;# OF SERVOS SCHEDULED
INITSV: MOV (R2)+,R1 ;PTR TO SERVO DATA BLOCK
BIS #RUN,-SPDBLK(R1) ;SET SERVO RUN BIT
MOV (R2)+,PTIME-SPDBLK(R1) ;SET START TIME OF SERVO
SOB R0,INITSV
CMP (R2)+,#FRCPBB ;SAVE FORCE LEVEL 7 TIME
BNE 1$
MOV (R2),FPTIME
MOV #RUN+FIRST,FSTAT;INDICATE FIRST PASS THROUGH
CLR FRCJTS
1$: EVWAIT (SP) ;WAIT FOR SERVOS TO FINISH
EVKIL ;DELETE EVENT NUMBER
TST BOVLCK ;CHECK FOR BOVER ON BLUE ARM
BEQ 2$
MOV #BOVERR,R0 ;INDICATE BACKGROUND JOB ERROR
2$: BIT #BARM+BHAND,R3 ;BLUE ARM
BEQ RUNDNE ;NO
DECDAC: DECB KICDAC ;STOP DAC REFRESH JOB
RUNDNE: MOV (SP)+,R1
MOV (SP)+,(R1)
MOV (SP)+,R2
MOV (SP)+,R3
MOV (SP)+,R4
RTS PC
;LOCAL STORAGE AREA
DATA
SCHBLK: .BLKW 33. ;LIST OF PDB'S OF SERVOS TO SCHEDULE
SCTIME: 10. ;TIME DELAY BEFORE FIRST SERVO SHOULD BE SCHED
CODE
;END OF "RUNSRV"
;DACSER - BLUE ARM DAC REFRESH LEVEL 4 ROUTINE AND ARM ERROR TRAP ROUTINE
DACSER: MOV #7,R0 ;NUMBER OF DACS TO REFRESH
MOV #BSRVOS,R1 ;BLUE ARM SERVO DATA BLKS
MOVB #20,DACCSR ;RESET HARDWARE TIME-OUT
1$: MOV (R1)+,R5 ;PTR TO DATA BLK
MOV #RUN+MOVING,R2 ;NEED TO REFRESH DAC?
BIC (R5),R2
BEQ 2$ ;NO
BIT #DACDNE,DACCSR ;DAC READY FOR NEW VALUE?
BEQ .-6 ;NO
MOV DACCHN(R5),DACCSR;REFRESH A DAC
2$: SOB R0,1$
TSTB KICDAC ;NEED TO KEEP RE-FRESHING?
BEQ 3$ ;NO
SLEEP #16.
BR DACSER
3$: BIC #KICRUN,KICDAC ;DISMISS FOREVER
BIS #10,174004 ;DISABLE BLUE ARM ALARM INTERRUPTS
DISMISS
;ARM ERROR INTERRUPT ROUTINE
ARMERR: BIS #1,BPANIC ;INDICATE BLUE ARM ERROR CONDITION
KRTI ;RETURN USING EMT IN KERNEL
YERR: BIS #2,BPANIC ;INDICATE YELLOW ARM ERROR CONDITION
KRTI ;RETURN VIA KERNEL
;LOCAL STORAGE AREA
DATA
BPANIC: 0 ;SET TO 1 IF PANIC BUTTON HIT
KICDAC: .WORD 0 ;DAC REFRESH STATUS BITS CONTAINS:
KICRUN==100000 ;RE-FRESH JOB RUNNING
; 377 ;≠0 MEANS DONT STOP RUNNING
YEMSG: .ASCIZ /YELLOW PANIC BUTTON PUSHED!/
MUGDAC: PDBLK 4,20
CODE
;END OF DAC REFRESH ROUTINE
; TOUCH - SCHEDULES TOUCH EVENTS
;THIS PROGRAM IS USED FOR SCHEDULING EVENTS THAT ARE TO BE TRIGGERED ACCORDING
;TO THE STATE OF A TOUCH SENSOR. THE EVENT SIGNALING CAN TAKE PLACE WHEN A
;TOUCH SENSOR READS ON OR OFF. AS MANY AS TWENTY-FIVE DIFFERENT EVENTS CAN
;BE WAITING FOR VARIOUS TOUCH SENSOR STATES. READING OF THE TOUCH SENSORS AND
;DECIDING WHICH EVENTS ARE TO BE ACTIVITED IS DONE AT LEVEL 7. THE LEVEL 7
;JOB IS SCHEDULED ONLY WHEN SOME EVENTS ARE PENDING. THE WORDS CONTAINING THE
;STATUS OF EACH TOUCH SENSOR ARE UPDATED EACH TIME THE LEVEL 7 JOB IS RUN.
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #EVENT,R0 ;EVENT TO BE TRIGGERED ON TOUCH
; MOV #SENSOR,R1 ;INDICATES TOUCH SENSOR AND STATE
; ; NEED TO TRIGGER EVENT
; JSR PC,TOUCH
;
;IF THE HIGH BYTE OF THE SENSOR NUMBER IS ZERO, THE OFF CONDITION WILL TRIGGER
;THE EVENT, ELSE THE ON CONDITION OF THE SENSOR WILL BE USED AS A TRIGGER.
;THE LOW BYTE OF THE "SENSOR" NUMBER MUST CONTAIN THE LOGICAL NUMBER FOR THE
;SENSOR TO BE TESTED. THE LOGICAL NUMBERS ASSIGNMENTS ARE AS FOLLOWS:
;
; SENSOR LOGICAL NUMBER
; BLUE HAND, RIGHT SIDE 0
; BLUE HAND, LEFT SIDE 1
; YELLOW HAND, RIGHT SIDE 2
; YELLOW HAND, LEFT SIDE 3
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR OCCURRED.
;THE ERROR CODES ARE LISTED ON PAGE 8.
;DEFINITIONS:
MAXTCH ==2 ;TOTAL NUMBER OF OPERATIONAL TOUCH SENSORS
TCHBLK ==25. ;NUMBER OF F.S. BLOCKS IN TOUCH LIST
TCHSCH ==400 ;LEVEL 7 STATUS BIT, INDICATES LEVEL 7 CODE SCHEDULED
RUNTCH ==1000 ; " " " " , ALWAYS RE-SCHEDULE LEVEL 7 CODE
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE ALTERED
TOUCH: MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R1,R2
BIC #177400,R1 ;GET SENSOR NUMBER
CMP #MAXTCH,R1 ;MUST BE LESS THAN THIS
BGT TCHOK
TCHERR: MOV #UNKTCH,R0 ;SIGNAL ERROR
BR TCHDNE
TCHOK: ASL R1 ;WANT WORD INDEX
ADD #TCHOFF,R1 ;GET PTR TO TRIGGER ON OFF STATE
BIT #177400,R2 ;CHECK IF TRIGGER USING ON OR OFF STATE
BEQ .+6
ADD #TCHON-TCHOFF,R1 ;GET PTR TO TRIGGER ON ON STATE
;PUT TOUCH EVENT IN EVENT LIST
INCB TCHUP ;LOCK OUT LEVEL 7 CODE FROM USING EVENT LIST
MOV LSTUSE,R3 ;START CHECKING FROM LAST SLOT USED
MOV #TCHBLK,R2 ;CHECK ALL EVENT SLOTS FOR A FREE ONE
1$: SUB #4,R3 ;CHECK NEXT SLOT
BGT .+6 ;WRAP AROUND IF AT BOTTOM OF LIST
MOV #TCHBLK*4-4,R3
TST EVTLST(R3) ;CHECK IF THIS EVENT SLOT FREE
BEQ 2$ ;BRANCH IF EMPTY
SOB R2,1$ ;CHECK ALL AVAILABLE SLOTS
MOV #TOMTCH,R0 ;NO EMPTY SLOTS, SIGNAL ERROR
BR 4$
2$: MOV R3,LSTUSE ;SAVE PTR TO LAST SLOT USED
ADD #EVTLST,R3 ;GET ABSOLUTE ADDRESS OF FREE SLOT
MOV R0,(R3) ;PUT IN EVENT TO BE TRIGGERED
MOV (R1),2(R3) ;SAVE PTR TO PREVIOUS EVENT TIED TO THIS
; TOUCH SENSOR CONDITION
MOV R3,(R1) ;SAVE PTR TO THIS EVENT
INC ACTNUM ;INCREASE THE NUMBER OF SLOTS NOW TAKEN
BIT #TCHSCH,TCHUP ;CHECK IF LEVEL 7 CODE SCHEDULED TO RUN
BNE 3$ ;BRANCH IF SCHEDULED
.IFZ SNGSTP
SCHED7 #TCHPPB,#1 ;SCHEDULE LEVEL 7 CODE TO READ TOUCH SENSORS
BIS #TCHSCH,TCHUP ;INDICATE LEVEL 7 CODE SCHEDULED
.ENDC
3$: CLR R0 ;INDICATE NO ERRORS
4$: DECB TCHUP ;ALLOW LEVEL 7 CODE TO USE EVENT LIST
;EXIT CLEANLY
TCHDNE: MOV (SP)+,R3 ;RESTORE REGISTERS
MOV (SP)+,R2
RTS PC
;END OF "TOUCH"
;CLRTCH - TAKES A SCHEDULED EVENT OFF OF THE TOUCH SENSOR EVENT LISTS
;THIS ROUTINE NEGATES THE ACTIONS TAKEN BY A CALL TO "TOUCH". A SAMPLE CALLING
;SEQUENCE TO "CLRTCH" FOLLOWS:
;
; MOV #EVENT,R0 ;EVENT TO BE TAKEN OFF LIST
; MOV #SENSOR,R1 ;INDICATES TOUCH SENSOR AND STATE
; ; WITH WHICH EVENT WAS ASSOCIATED
; JSR PC,CLRTCH
;
;THE INTERPRETATION OF THE "EVENT" AND "SENSOR" NUMBERS IS THE SAME AS IN
;"TOUCH". AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO
;ERROR OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8. IF NO TOUCH EVENT
;WAS FOUND, THE NORMAL ERROR RETURN IS TAKEN WITH NO ERROR INDICATED.
;REGISTERS USED:
;
; R0, R1 PASS ARGUMENTS AND ARE ALTERED
CLRTCH: MOV R2,-(SP) ;SAVE REGISTER
MOV R1,R2
BIC #177400,R1 ;GET SENSOR NUMBER
CMP #MAXTCH,R1 ;MUST BE LESS THAN THIS
BGT 1$
MOV #UNKTCH,R0 ;SIGNAL ERROR
BR CTHDNE
1$: ASL R1 ;WANT WORD INDEX
ADD #TCHOFF,R1 ;GET PTR TO TRIGGER ON OFF STATE
BIT #400,R2 ;CHECK IF TRIGGER USING ON OR OFF STATE
BEQ .+6
ADD #TCHON-TCHOFF,R1 ;GET PTR TO TRIGGER ON ON STATE
;TAKE TOUCH EVENT OFF EVENT LIST
INCB TCHUP ;LOCK OUT LEVEL 7 CODE FROM USING EVENT LIST
2$: TST (R1) ;END OF LIST?
BEQ CTEOL ;BRANCH IF NO EVENT MATCH FOUND
MOV (R1),R2 ;ELSE GET POINTER TO EVENT
CMP R0,(R2)+ ;MATCHING EVENT?
BEQ GOTEVT ;BRANCH IF WE FOUND IT
MOV R2,R1 ;ELSE GET POINTER TO NEXT EVENT ON LIST
BR 2$ ;CHECK NEXT EVENT
GOTEVT: MOV (R2),(R1) ;SKIP OVER REQUESTED EVENT
CLR -2(R2) ;FREE EVENT SLOT
DEC ACTNUM ;INDICATE ONE LESS EVENT PENDING
CTEOL: DECB TCHUP ;UNLOCK LEVEL 7 CODE
CLR R0 ;INDICATE NO ERRORS
;EXIT CLEANLY
CTHDNE: MOV (SP)+,R2 ;RESTORE REGISTER
RTS PC
;END OF "CLRTCH"
;SMPTCH,STPTCH - INITIATES AND TERMINATES UPDATING OF THE TOUCH SENSORS READINGS
;"SMPTCH" ENSURES THAT THE TOUCH SENSOR STATUS WORDS WILL BE UPDATED PERIODLY
;WHETHER OR NOT ANY TOUCH SENSOR EVENTS ARE PENDING. A SAMPLE CALLING SEQUENCE
;TO THIS ROUTINE FOLLOWS:
;
; JSR PC,SMPTCH ;NO ARGUMENTS
;
;SUCCESS IS ALWAYS GUARANTEED, HENSE NO ERROR CODE IS RETURNED
;REGISTERS USED: NONE
SMPTCH: INCB TCHUP ;LOCKOUT LEVEL 7 CODE
BIS #RUNTCH,TCHUP ;TELL LEVEL 7 CODE TO RUN CONTINUOUSLY
BIT #TCHSCH,TCHUP ;CHECK IF LEVEL 7 CODE SCHEDULED TO RUN
BEQ 1$ ;BRANCH IF SCHEDULED
.IFZ SNGSTP
SCHED7 #TCHPPB,#1 ;SCHEDULE LEVEL 7 CODE TO READ TOUCH SENSORS
BIS #TCHSCH,TCHUP ;INDICATE LEVEL 7 CODE SCHEDULED
.ENDC
1$: DECB TCHUP ;UNLOCK LEVEL 7 CODE
RTS PC
;"STPTCH" RETURNS THE LEVEL 7 CODE TO MODE OF OPERATION OF ONLY RUNNING IF
;THERE IS A PENDING TOUCH SENSOR EVENT IN THE EVENT LIST. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; JSR PC,STPTCH ;NO ARGUMENTS
;
;SUCCESS IS ALWAYS GUARANTEED, HENSE NO ERROR CODE IS RETURNED
;REGISTERS USED: NONE
STPTCH: BIC #RUNTCH,TCHUP ;RETURN TO STANDARD MODE
RTS PC
;END OF "SMPTCH" AND "STPTCH"
;TCHLV7 - LEVEL 7 CODE FOR TOUCH SENSORS
;THIS ROUTINE UPDATES THE STATUS WORDS FOR THE TOUCH SENSORS ( TCHSTS ) WHEN
;IT IS RUN SO LONG AS THE LOCKOUT BYTE OF THE STATUS WORD ( TCHUP ) IS ZERO.
;IF UPDATING IS PERMITTED, ANY EVENTS IN THE EVENT LIST WHOSE CONDITION IS
;NOW TRUE ARE SIGNALLED. THIS ROUTINE RE-SCHEDULES ITSELF IF AT LEAST
;ONE EVENT IS PENDING OR IF THE "RUNTCH" BIT OF THE "TCHUP" WORD IS ON.
;IF NOT LOCKED OUT, UPDATE SENSORS AND SIGNAL TRUE CONDITION EVENTS
TCHLV7: TSTB TCHUP ;CHECK IF LOCKED OUT
BNE SCHTCH ;LOCKED OUT, JUST GO RE-SCHEDULE TO RUN AGAIN
MOV #TCHADL,AC ;PTR TO ADC CHANNELS TO READ
MOV #TCHVAL,BC ;PTR TO ARRAY TO STORE ADC READINGS
MOV #TCHOFF,CC ;PTR TO "OFF" CONDITION EVENTS
MOV #TCHON,DC ;PTR TO "ON" CONDITION EVENTS
MOV #TCHSTS,EC ;PTR TO ON/OFF STATUS WORDS FOR CHANNELS
MOVB (AC)+,ADCCHN ;START ADC WORKING ON FIRST CHANNEL
MOV #TLVEXT,-(SP) ;SAVE ADDRESS OF EXIT SECTION
TLV7LP: TSTB ADCSR ;WAIT TILL CONVERSION COMPLETED
BPL .-4
MOV ADCVAL,(BC) ;SAVE ADC READING
MOVB (AC),ADCCHN ;START CONVERTING NEXT CHANNEL
CMP SENZ0-TCHVAL(BC),(BC)+ ;CHECK SENSOR VALUE
BLT 1$ ;BRANCH IF SENSOR ABOVE ZERO LEVEL
CLR (EC)+ ;INDICATE SENSOR OFF
MOV (CC),DAT ;WANT PTR TO SIGNAL "OFF" CONDITION EVENTS
CLR (CC)+ ;INDICATE NO MORE EVENTS PENDING ON THIS CONDITION
TST (DC)+ ;UPDATE "ON" EVENT POINTERS
BR 2$
1$: MOV #1,(EC)+ ;INDICATE SENSOR ON
MOV (DC),DAT ;WANT PTR TO "ON" CONDITION EVENTS
CLR (DC)+
TST (CC)+ ;UPDATE "OFF" EVENT POINTER
2$: TST DAT ;CHECK IF ANY EVENTS PENDING
BEQ NXTSEN ;IF NONE, SKIP TO NEXT SENSOR
3$: MOV (DAT),-(SP) ;WE WILL SIGNAL THIS LATER SINCE THE KERNEL WOULD
MOV #SGNEVT,-(SP) ; CLOBBER OUR REG. IF WE DID IT NOW
CLR (DAT)+ ;FREE EVENT LIST SLOT
DEC ACTNUM ;INDICATE ONE LESS EVENT PENDING
MOV (DAT),DAT ;GET PTR TO NEXT EVENT ON LIST
BNE 3$ ;REPEAT FOR ALL EVENTS ATTACHED TO THIS CONDITION
NXTSEN: TSTB (AC)+ ;CHECK IF END OF ADC LIST
BPL TLV7LP ;REPEAT TILL DONE
BR TSTSIG
SGNEVT:
.IFZ SNGSTP
EVSIG ;SIGNAL AN EVENT THAT OCCURRED
.ENDC
TSTSIG: RTS PC ;THIS BRANCHS TO SGNEVT IF WE SAVED ANY EVENTS TO
TLVEXT: ; SIGNAL, OTHERWISE IT COMES HERE
TST ACTNUM ;CHECK IF ANY EVENTS STILL PENDING
BNE SCHTCH ;BRANCH IF STILL MORE
BIT #RUNTCH,TCHUP ;ELSE CHECK IF WE ARE TO RUN CONTINUOUSLY
BEQ TLVKIL ;IF NOT, NO MORE TO DO, KILL LEVEL 7 CODE
;RE-SCHEDULE TO RUN AGAIN
SCHTCH:
.IFZ SNGSTP
RESCH7 #16. ;RE-SCHEDULE LEVEL 7 CODE TO READ TOUCH SEN.
TST (SP)+ ;CLEAR SCHEDULE TIME OFF STACK
DISMS7
.ENDC
;DON'T RE-SCHDULE
TLVKIL:
.IFZ SNGSTP
BIC #TCHSCH,TCHUP ;INDICATE LEVEL 7 NOT SCHEDULED AGAIN
DISMS7
.ENDC
;TOUCH SENSOR SERVO DATA BLOCK
DATA
TCHUP: 0 ;LEVEL 7 STATUS BITS
ACTNUM: 0 ;NUMBER OF PENDING EVENTS
;DATA FOR USING BINARY FINGERS
.IFZ FFING
TCHADL: .BYTE 0,1,377,0 ;TOUCH SENSOR A/D CHANNELS
SENZ0: 450. ;TRANSITION READING FOR BINARY FINGERS
450.
.ENDC
;DATA FOR USING FORCE SENSING FINGERS
.IFNZ FFING
TCHADL: .BYTE 32.,33.,377,0 ;TOUCH SENSOR A/D CHANNELS
SENZ0: 100 ;TRANSITION READING FOR FORCE FINGERS
100
.ENDC
;TCHVAL TO LSTUSE IS ZEROED BY "INTARM"
TCHVAL: .BLKW MAXTCH ;A/D TOUCH SENSOR READINGS
TCHSTS: .BLKW MAXTCH ;TOUCH SENSOR LOGICAL STATUS ( TRUE OR FALSE )
TCHON: .BLKW MAXTCH ;PTR TO EVENTS TO TRIGGER IF TOUCH SENSOR ON
TCHOFF: .BLKW MAXTCH ; " " " " " " " " OFF
EVTLST: .BLKW TCHBLK*2;STORAGE FOR EVENTS TO BE TRIGGERED
LSTUSE: TCHBLK*4 ;REL. POINTER TO LAST SLOT USED IN EVENT LIST
;TOUCH SENSOR LEVEL 7 CODE PROCESS DESCRIPTOR BLOCK
TCHPPB: TCHPBB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0 ;TIME AND END OF LIST
TCHPBB:
.IFZ SNGSTP
PDBLK7 TCHLV7,0,20
.ENDC
CODE
;END OF "TCHLV7"
; FORCE - SUBRS. USED FOR FORCE SENSING AND COMPLIANCE
FRATE ==5 ;NUMBER OF TIMES FRCLV7 IS EXECUTED BEFORE A CALL TO FBACK
WRSPLS ==40 ;FORCE WRIST PULSE BIT, TO RESET MULTIPLEXER
WRSCHN ==66 ;FORCE WRIST ADC CHANNEL
FRCBLK ==15. ;NUMBER OF FORCE JOB THAT CAN BE PENDING
;THE FOLLOWING MECHANISM BITS INDICATE WHICH PHYSICAL DEVICE IS BEING
;USED:
YELARM ==1 ;YELLOW ARM, NOT INCLUDING HAND
BLUARM ==4 ;BLUE ARM, NOT INCLUDING HAND
;THE FOLLOWING BITS ARE USED DURING CALLS TO THE FORCE SENSING SYSTEM AND
;BY THE FORCE SENSING SYSTEM ITSELF TO SPECIFY RUN-TIME OPTIONS.
FTABLE ==400 ;FORCE TRANS (C) IN TABLE COORDINATES
FHAND ==0 ; " " " " HAND COORDINATE SYSTEM
XFORCE ==0 ;FORCE IN X DIRECTION OF C
YFORCE ==1000 ; " " Y " " "
ZFORCE ==2000 ; " " Z " " "
XMOMENT ==3000 ;MOMENT ABOUT X DIRECTION OF C
YMOMENT ==4000 ; " " Y " " "
ZMOMENT ==5000 ; " " Z " " "
FSTOP ==10000 ;IN ADDITION TO SPROUTING JOB, STOP ARM
SIGGE ==100000;START JOB IF FORCE ≥ SPECIFIED VALUE
SIGLT ==0 ; " " " " < " "
SIGMAG == 20000;TEST ONLY MAGNITUDE OF FORCES
;SUBR TO VERIFY SAME ARM SPECIFIED BY FORCE ROUTINES
SAMARM: MOV R0,R3 ;SAME ARM SPECIFIED?
BIC #-1#BLUARM#YELARM,R3
BIT R3,@FCARAY
BNE 1$
MOV #WRGARM,R0 ;NO, SIGNAL ERROR
SEV
1$: RTS PC
;SETC - INITIALIZES FORCE SENSING/COMPLIANCE SYSTEM
.IFZ DIAGY
;THIS ROUTINE IS USED FOR SPECIFYING THE FORCE COORDINATE SYSTEM TOGETHER
;WITH THE MANIPULATOR THAT IS TO BE FORCE SERVOED AND FOR INITIALIZING
;THE FORCE SENSING/COMPLIANCE ROUTINES. IT SHOULD BE CALLED AT LEAST
;ONCE BEFORE THE START OF EACH MOTION THAT REQUIRES FORCE SENSING/COMPLIANCE.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;BLUARM/YELARM AND TABLE/HAND BITS
; MOV #C,R1 ;PTR TO FORCE TRANS
; JSR PC,SETC
;
;IF THE FORCE SENSING SYSTEM IS CURRENTLY ACTIVE AND THE ARM SPECIFIED
;IN R0 IS DIFFERENT FROM THE ARM CURRENTLY IN USE, THIS INSTRUCTION IS
;ABORTED AND AN ERROR CODE IS RETURNED IN R0, ELSE R0 IS SET TO ZERO.
;**NOTE** IF THE FORCE SENSING SYSTEM IS NOT ACTIVE AT THE TIME THAT
;THIS ROUTINE IS CALLED, THE ACTION OF ALL PREVIOUS "COMPLY" COMMANDS
;IS UN-DONE.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE GARBAGED
SETC: MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
BIT #RUN,FSTAT ;FORCE SYSTEM ACTIVE?
BEQ 1$ ;NO
JSR PC,SAMARM ;YES, SAME ARM?
BVS SETCDN ;NO
BR 5$
1$: CLR FRCJTS ;SIGNAL NO JTS FORCE SERVOING
CLR CMPCPN ;FORCE COMPLIANCE ONLY GOOD ONCE
MOV #OTCN,R2 ;INITIALIZE FORCE TRANS
MOV #TCP2,R3
MOV #IMAT,R4
MOV #12.*2,R5
2$: MOV (R4),(R2)+
MOV (R4),TTRAN-IMAT(R4)
MOV (R4)+,(R3)+
SOB R5,2$
CLRF OP6
CLRF OP6+4
CLRF OP6+10
MOV #TTHC,TTHCP ;SET ANGLE ARRAY PTR
MOV #TTH,TTHP
MOV #SERVOS,R2 ;PTRS TO SERVO DATA BLOCKS
BIT #BLUARM,R0 ;BLUE ARM?
BEQ 3$
ADD #7*2,R2 ;YES
SUB #14.,TTHCP
SUB #14.,TTHP
3$: MOV R2,SRVPTR ;SAVE PTR
MOV #TTHC1,R4 ;INITIALIZE ANGLES TO CURRENT VALUES
MOV #6,R5
4$: MOV (R2)+,R3 ;SERVO DATA BLOCK
MOV TH(R3),(R4)+ ;TRANSFER CURRENT JOINT ANGLE
MOV TH+2(R3),(R4)+
SOB R5,4$
5$: MOV FCARAY,R2 ;PTR TO CURRENT C TRANS AND BITS
MOV NEXTC(R2),R2 ;PTR TO NEXT TRANS/BITS DATA AREA
MOV R2,R3
MOV R0,(R2)+ ;SAVE ARM/MODE BITS
MOV #12.*2,R0 ;TRANSFER C TRANSFORM
MOV (R1)+,(R2)+
SOB R0,.-2
MOV R3,FCARAY ;START USING NEW C ARRAY AND BITS
mov #6,r2
mov #biasf,r3
6$: clrf #<6*4>(r3) ;ZERO ALL BIAS FORCES
clrf (r3)+
sob r2,6$
SETCDN: MOV (SP)+,R5
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
;END OF "SETC"
;FRCSIG - INITIALIZES JOB STARTING BASED UPON A FORCE READING
;THIS RTN. IS USED FOR SPROUTING JOBS THAT ARE TO BE TRIGGERED ACCORDING
;TO THE VALUE OF A FORCE COMPONENT. THE SPROUTING CAN TAKE PLACE WHEN
;A FORCE LEVEL EITHER EXCEEDS OR DROPS BELOW A SPECIFIED MAGNITUDE. THE ONLY
;CONSTRAINTS ARE THAT THE FORCE COMPONENT TO BE CHECKED MUST COINCIDE WITH
;ONE OF THE CARDINAL AXES OF THE CURRENT FORCE TRANSFORM( SEE "SETC") AND THE
;ARM SPECIFIED MUST BE THE SAME AS THAT SPECIFIED IN THE LAST "SETC" CALL. AS
;MANY AS "FRCBLK" DIFFERENT JOBS CAN BE WAITING FOR VARIOUS FORCE LEVELS. THE
;EVALUATION OF THE CURRENT FORCE LEVELS ARE DONE AT LEVEL 7 AND A SUBSIDIARY
;JOB IS RUN AT LEVEL 5. BOTH OF THESE JOBS ARE RUN ONLY WHEN SPROUTING IS
;PENDING OR SOME FORCE COMPLIANCE TASK IS REQUESTED.
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SEE BELOW
; MOV #PDB,R1 ;PDB OF JOB TO BE TRIGGERED ON FORCE
; MOV #ADDR,R2 ;STARTING ADDR OF SPROUTING JOB
; LDF VAL,AC0 ;FORCE VALUE IN OZ OR OZ-IN
; JSR PC,FRCSIG ;***WARNING: NEVER CALL THIS ROUTINE
; ; ABOVE CPU LEVEL 4***
;
;THE CONTENTS OF THE "BITS" IN R0 ARE USED FOR SPECIFYING ONE OPTION FROM
;EACH OF THE FOLLOWING GROUPS:
;
; ARM: YELLOW/BLUE
; FORCE COMPONENT TO CHECK: FORCE X/Y/Z MOMENT X/Y/Z
; START JOB IF: FORCE ≥/< SPECIFIED VALUE
; STOP ARM WHEN JOB STARTED: YES/NO
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR IN
;QUEUING THE JOB OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0,R1,R2,AC0 PASS ARGUMENTS AND R0 IS ALTERED
FRCSIG: MOV R3,-(SP) ;SAVE REGISTERS
MOV R4,-(SP)
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS FRCDNE ;NO
MOV R0,R3 ;GET PTR INTO FORCE COMPONENT LIST
BIC #170777,R3
SWAB R3
;PUT FORCE JOB IN QUEUE
INCB FSTAT+1 ;LOCK OUT LEVEL 7 CODE FROM USING JOB LIST
MOV FRCPTR,R4 ;ANY FREE JOB BLOCKS?
BNE 1$ ;YES
MOV #TOMFRC,R0 ;NO, SIGNAL ERROR AND ABORT
BR 2$
1$: MOV (R4),FRCPTR ;TAKE BLOCK OUT OF FREE CHAIN
MOV XFPTR(R3),(R4) ;PUT BLOCK IN COMPONENT CHAIN
MOV R4,XFPTR(R3)
TST (R4)+ ;SAVE JOB DATA IN BLOCK
BIS #SIGMAG,R0 ;INSURE MAGNITUDE CHECKING(remove when parser knows this)
BIT #SIGMAG,R0 ;MAGNITUDE ONLY?
BEQ 3$ ;NO
ABSF AC0 ;YES, ALLOW ONLY POSITIVE THRESHOLDS
3$: STF AC0,(R4)+ ;FORCE VALUE
BIC #FTABLE,R0 ;ONLY HAND COORDS AVAILABLE
MOV R0,(R4)+ ;BITS
MOV #3,(R4)+ ;TEST COUNTER
MOV R2,(R4)+ ;SPROUTING JOB
MOV R1,(R4)
INC FRCNUM ;INDICATE ONE MORE JOB PENDING
CLR R0 ;INDICATE NO ERRORS
2$: DECB FSTAT+1 ;ALLOW LEVEL 7 CODE TO USE JOB LIST
;EXIT CLEANLY
FRCDNE: MOV (SP)+,R4 ;RESTORE REGISTERS
MOV (SP)+,R3
RTS PC
;END OF "FRCSIG"
;FRCOFF - TAKES A QUEUED JOB OFF OF THE FORCE SIGNAL LIST
;THIS ROUTINE NEGATES THE ACTIONS TAKEN BY A CALL TO "FRCSIG". A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SAME TWO ARGS AS IN "FRCSIG"
; MOV #PDB,R1
; JSR PC,FRCOFF
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR IN
;DELETING THE JOB OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0, R1 PASS ARGUMENTS AND ARE ALTERED
FRCOFF: MOV R3,-(SP) ;SAVE REGISTER
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS 5$ ;NO
BIC #170777,R0 ;GET PTR INTO FORCE COMPONENT LIST
SWAB R0
ADD #XFPTR,R0 ;PTR TO SPECIFIED COMPONENT LIST
INCB FSTAT+1 ;LOCK OUT LEVEL 7 CODE FROM USING JOB LIST
MOV (R0),R3 ;FIRST JOB IN QUEUE
;TAKE FORCE JOB OUT OF QUEUE
1$: CMP 14(R3),R1 ;FOUND RIGHT PDB?
BEQ 3$ ;YES
MOV (R3),R3 ;NEXT JOB IN QUEUE
BNE 1$
MOV #1,R0 ;COULDN'T FIND REQUESTED JOB
BR 4$ ;NO MORE JOBS TO CHECK
3$: MOV (R3),(R0) ;REMOVE JOB FROM QUEUE
MOV FRCPTR,(R3)
MOV R3,FRCPTR
DEC FRCNUM ;INDICATE ONE LESS JOB PENDING
CLR R0 ;SIGNAL NO ERROR
4$: DECB FSTAT+1 ;ALLOW LEVEL 7 CODE TO USE JOB LIST
5$: MOV (SP)+,R3
RTS PC
;END OF "FRCOFF"
;BISON - SETS UP BIAS FORCE OF A GIVEN MAGNITUDE AND DIRECTION
;
;THIS IS JUST LIKE "FRCSIG" EXCEPT THAT INSTEAD OF QUEUING JOBS WAITING
;ON FORCE LEVELS, THIS ROUTINE INITIALIZES FORCE COMPLIANCE IN A GIVEN
;CARDINAL DIRECTION OF THE CURRENT FORCE TRANSFORMATION ( SEE "SETC" )
;AND AT A GIVEN FORCE MAGNITUDE. (routine currently defaults to hand frame)
;AS WITH "FRCSIG" THE SPECIFIED ARM
;MOST COINCIDE WITH THE ARM SPECIFIED IN THE LAST CALL TO "SETC" OR
;AN ERROR MESSAGE IS RETURNED. NATURALLY, ONLY ONE BIAS LEVEL
;CAN BE SET IN ANY SINGLE DIRECTION AND SO ONLY THE MOST RECENT FORCE
;LEVELS ARE RETAINED. THIS ROUTINE SHARES A LEVEL 7 AND A SUBSIDIARY
;LEVEL 5 JOB WITH "FRCSIG".
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SEE BELOW
; LDF VAL,AC0 ;FORCE VALUE IN OZ OR OZ-IN
; JSR PC,COMPLY ;***WARNING: NEVER CALL THIS ROUTINE
; ; ABOVE CPU LEVEL 4***
;
;THE CONTENTS OF THE "BITS" IN R0 ARE USED FOR SPECIFYING ONE OPTION FROM
;EACH OF THE FOLLOWING GROUPS:
;
; ARM: YELLOW/BLUE
; FORCE COMPONENT TO APPLY: FORCE X/Y/Z MOMENT X/Y/Z
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR
;OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;REGISTERS USED:
; R0,AC0 PASS ARGUMENTS AND R0,R1 ARE ALTERED
;OLD COMPLY ROUTINE FLUSHED TO OLDFRC.PAL[1,JKS] ON 14-AUG-79
BISON: MOV R3,-(SP)
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS 1$
BIC #170777,R0 ;PTR INTO COMPLIANCE LIST
SWAB R0
ASL R0
STF AC0,biasf(R0) ;SAVE COMPLIANCE LEVEL
CLR R0 ;INDICATE NO ERRORS
1$: MOV (SP)+,R3
RTS PC
;END OF "BISON"
;BISOFF - TURNS OFF FORCE COMPLIANCE IN A SPECIFIED DIRECTION
;
;THIS ROUTINE NEGATES THE ACTIONS TAKEN BY A CALL TO "BISON". A SAMPLE
;CALLING SEQUENCE FOLLOWS:
;
; MOV #BITS,R0 ;SAME ARG AS IN "COMPLY"
; JSR PC,CMPOFF
;
;AFTER EXECUTION, R0 CONTAINS AN ERROR CODE AND IS ZERO IF NO ERROR IN
;DELETING THE JOB OCCURRED. THE ERROR CODES ARE LISTED ON PAGE 8.
;(OLD CMPOFF ROUTINE FLUSHED TO OLDFRC.PAL[1,JKS] ON 14-AUG-79)
BISOFF: MOV R3,-(SP)
JSR PC,SAMARM ;SAME ARM SPECIFIED?
BVS 1$
BIC #170777,R0 ;PTR INTO COMPLIANCE LIST
SWAB R0
ASL R0
CLRF biasf(R0) ;CLEAR BIAS FORCE
CLR R0 ;INDICATE NO ERRORS
1$: MOV (SP)+,R3
RTS PC
;END OF BISOFF
;FRCLV7 - LEVEL 7 CODE FOR FORCE SENSING AND COMPLIANCE
;AT THE START OF EXECUTION OF THIS ROUTINE, DAT ← #XFPTR
FRCLV7: TST @FINUSE ;CATASTROPHIC ERROR TERMINATION?
BPL .+6
JMP KILFRC ;YES
TST STFDAT ;HAVE WE ALREADY READ THE GAGES?
BNE 1$
JSR PC,REPS ;READ GAGES
1$: TST GDATA ;ARE WE GATHERING?
BEQ 2$ ;NO
JSR PC,RNS ;YES RESOLVE AND STORE INFO.
2$: TST FRCNUM ;ANY FORCE SENSING TO DO?
BEQ CHKDIE ;NO
BIT #177400+FIRST,FSTAT;1ST PASS OR LOCKED OUT FROM USING JOB LIST?
BNE RESFRC ;YES
MOV #STDCAL,BC ;POINT TO WRIST CALIBRATION MATRIX
MOV #6,EC ;# OF FORCE COMPONENTS TO CHECK
MOV #FRCEXT,-(SP) ;"RTS" THIS TO EXIT
FRCLOP: MOV (DAT),DC ;ANY JOBS PENDING ON THIS COMPONENT?
BEQ NOFJOB
MOV #EPSF,AC ;point to table of strain gage readings
CLRF AC0
MOV #8.,CC ;COMPUTE force value = jacob*epsf
1$: LDF (AC)+,AC1 ;get gage reading
MULF (BC),AC1 ;JACOBIAN COMPONENT
ADD #6*4,BC ;POINT TO NEXT ITEM IN COL.
ADDF AC1,AC0 ;DOT PRODUCT
SOB CC,1$
SUB #<8.*6*4>,BC ;POINT TO NEXT COL
MOV DAT,CC ;CHECK IF ANY PENDING JOBS TO START UP
CHKJOB: TST (DC)+
STF AC0,AC1
BIT #SIGMAG,4(DC) ;CHECKING ONLY MAGNITUDE?
BEQ 3$ ;NO
ABSF AC1 ;GET MAGNITUDE
3$: CMPF (DC)+,AC1 ;CHECK VALUE
TST (DC)+ ;SPROUT ON FORCE ≥ VAL OR < VAL?
BPL 1$
CFCC ;VAL ≤ FORCE?
BLE 2$ ;YES
BR 5$ ;NO
1$: CFCC ;VAL > FORCE?
BLE 5$ ;NO
2$: DEC (DC) ;FILTER, DONT STOP UNLESS TEST OK 3 TIMES
BGT 6$
BIT #FSTOP,-(DC) ;STOP ARM MOTION?
BEQ .+10
BIS #100000,@FINUSE ;YES
CMP (DC)+,(DC)+
MOV #USRDM,-(SP) ;WE WILL SPROUT THIS JOB LATER SINCE THE KERNEL
MOV (DC)+,-(SP) ; WOULD CLOBBER OUR REG. IF WE DID IT NOW
MOV (DC),-(SP) ; WOULD CLOBBER OUR REG. IF WE DID IT NOW
MOV #SPRTIT,-(SP)
MOV (CC),DC ;RETURN THE DATA BLOCK TO FREE STORAGE LIST
MOV (DC),(CC)
MOV FRCPTR,(DC)
MOV DC,FRCPTR
DEC FRCNUM ;INDICATE ONE LESS JOB PENDING
BR 7$
5$: MOV #3,(DC) ;RESET TEST COUNT
6$: MOV (CC),CC
7$: MOV (CC),DC ;CHECK NEXT JOB IN FORCE LIST
BNE CHKJOB
NOFJOB: TST (DAT)+ ;ON TO NEXT FORCE COMPONENT
ADD #4,BC ;POINT TO NEXT COL
DEC EC
BGT FRCLOP
; SOB EC,FRCLOP
BR SIGFRC
SPRTIT: FORK ;SPROUT FORCE JOB
SIGFRC: RTS PC ;THIS BRANCHES TO SPRTIT IF WE SAVED ANY JOBS TO
FRCEXT: ; SPROUT, OTHERWISE IT COMES HERE
;EXIT FROM LEVEL7 JOB ONE OF THREE WAYS:
CHKDIE: tst gdata
bne resfrc
BIT #FINAL,FSTAT ;TIME TO DIE?
BNE KILFRC ;YES
RESFRC: RESCH7 #16. ;RE-SCHEDULE LEVEL 7 CODE
SUB #16.,(SP) ;SAVE DIFF. BETWEEN PREDICTED AND ACTUAL TIME
ADD (SP)+,FPTIME
BIT #FIRST,FSTAT ;1ST TIME THROUGH?
BNE 1$ ;YES
DEC BCKTME ;TIME TO SCHEDULE BACKGROUND JOB?
BGT FRCFIN ;NO
1$: BIT #BACKG,FSTAT ;BACKG JOB NOT YET THROUGH RUNNING?
BNE 2$ ;YES, BIG TROUBLE
MOV #FRATE,BCKTME ;RUN BACKGROUND JOB EVERY 3RD TIME
BIC #FIRST,FSTAT
FORK7 #FRCPD5,#FBACK,#USRDM ;START UP BCK JOB
2$: BIS #100000,@FINUSE ;SIGNAL CAT. ERROR
INC BOVLCK ;indicate overrun condition
FORK7 #BOVPDB,#BOVER,#USRDM ;SIGNAL BACKG. JOB NOT DONE
KILFRC: CLRB FSTAT ;CLEAR ALL RUN FLAGS
CLR CMPCPN
CLR FRCJTS
FRCFIN: DISMS7
;BACKGROUND JOB OVER-RUN ERROR MESSAGE JOB
BOVER:
.IFZ STDALN
ALERR BOVERM ;TELL OPERATOR, BACKGROUND JOB TOOK TOO LONG
.IFF
MOV #BOVERM,SG
JSR PC,TYPSTR
.ENDC
DEC BOVLCK ;OPEN LOCK
DISMISS
DATA
BOVPDB: PDBLK 6,30
BOVERM: .ASCIZ /BACKGROUND FORCE SYSTEM JOB TOOK TOO LONG TO EXECUTE.
YOU'RE IN BIG TROUBLE NOW, CALL FOR HELP.
/
.EVEN
CODE
;END OF FRCLV7
;FBACK - LEVEL 5 FORCE SENSING BACKGROUND JOB
;THE FOLLOWING JOB IS STARTED AFTER EVERY 3RD EXECUTION OF THE LEVEL 7
;FORCE SENSING JOB, "FRCLV7". IT RECOMPUTES JACOBIAN AND STIFFNESS MATRICES
:AS NEEDED FOR FORCE SENSING, STIFFNESS CONTROL AND FORCE DATA GATHERING.
;THIS ROUTINE ;SHOULD ONLY BE EXECUTED BY DOING EITHER A "FORK", "FORK7",
;OR A "SCHEDU" THROUGH THE MONITOR.
FBACK: BIS #BACKG,FSTAT ;INDICATE BACKGROUND JOB RUNNING
TST STFDAT ;CHECK IF FORCE SERVOING
BEQ 12$ ;BR IF NOT
MOV #IMAT,R0 ;POINTER TO FORCE TRANS
MOV #THPTR,R1 ;POINT TO LIST OF ALL JOINT ANGLES
MOV #BLUARM+FHAND,R2;INDICATE BLUE ARM IN HAND COORDS
MOV #JC,R3 ;POINT TO RESULT MATRIX
JSR PC,GJTCAL ;COMPUTE JOINT TORQUE CALIBRATION MAT.
MOV RELX,R1 ;POINT TO RELATIVE XFORM TO STIFFNESS CENTER
JSR PC,GETKTH ;COMPUTE STIFFNESS MATRIX
;need to compute jacob in sta coords if so indicated
MOV #JC,BIASJA ;ASSUME IN BIAS FORCES IN HAND COORDS
; BIT #STACOORDS,SOMEWHERE
; BEQ 7$
; MOV #IMAT,R0 ;POINTER TO FORCE TRANS
; MOV #THPTR,R1 ;POINT TO LIST OF ALL JOINT ANGLES
; MOV #BLUARM+FSTA,R2 ;INDICATE BLUE ARM IN STATION COORDS
; MOV #JCSTA,R3 ;POINT TO RESULT MATRIX
; JSR PC,JACOB ;COMPUTE JACOBIAN
; MOV #JCSTA,BIASJA ;USE JACOBIN IN STA CCORDS
;COMPUTE BIAS TORQUES FROM BIAS FORCE
7$: MOV #BIAST,R0
MOV BIASJA,R1
MOV #6,R4
10$: MOV #BIASF,R2
MOV #6,R3
CLRF AC0
11$: LDF (R2)+,AC1
MULF (R1)+,AC1
ADDF AC1,AC0
SOB R3,11$
STF AC0,(R0)+
SOB R4,10$
; MOV #BTH,R0 ;COMPUTE CURRENT DYNAMIC COEF
; MOV #BCI,R1 ;UPDATE SERVO DATA BLOCKS
; MOV #BLUARM,R2 ;INDICATE BLUE ARM
; JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
12$: BIT #NXTFIN,FSTAT ;FINAL?
BNE 77$ ;YES
MOV FDATPT,R0 ;PTR TO START OF POLY. DATA LIST
ADD #16.*FRATE,FPTIME;GET NEXT LEVEL 5 TIME
MOV FPTIME,R5
MOV FTTME,R4 ;PRESENT TOTAL SEGMENT TIME
BIT #PASSGO,FSTAT ;WAITING TO START?
BNE 1$ ;NO
ADD #16.*FRATE,R5 ;P+2 TIME PAST START WAIT?
SUB R4,R5
BGE 5$ ;YES, GO START FORCE SERVOING
BR .+6
77$: INC FSTAT ;INDICATE "FINAL" PART OF MOTION
CLR FRCJTS ;DONT PERMIT FORCE SERVOING
JMP FBCKDN
1$: CMP R5,R4 ;PAST END OF SEGMENT?
BLT 6$ ;NO
2$: MOV (R0),R3 ;YES,SWITCH TO A NEW SEGMENT
ADD R3,R0 ;MOVE PTR
TST (R0) ;FINAL SEGMENT?
BNE 4$ ;NO
BIS #NXTFIN,FSTAT ;INDICATE P+2 IS END OF POLYS
CLRB NEWJTS ;DONT DO ANY MORE FORCE SERVOING AFTER P+1
BR COMTRN
4$: MOV R0,FDATPT ;NEW SEGMENT POINTER
ADD R3,FPOLY
SUB R4,R5 ;TIME INTO NEW SEGMENT
5$: MOV SEGTME(R0),R4 ;TOTAL TIME OF NEW SEG
LDCIF R4,AC0
CMP R5,R4 ;PAST END OF NEW SEGMENT?
BGE 2$ ;YES
MOV R4,FTTME ;NEW TOTAL SEGMENT TIME
MOV R5,FPTIME ;ADJUSTED PRESENT TIME
STF AC0,FFTIME
6$:
COMTRN:
;EXIT CLEANLY
ISGO: BIS #PASSGO,FSTAT ;YES, WE CAN START COMPUTING
FBCKDN:
.IFNZ FRCDAT
MOV FDATA,R0
MOV FSTAT,(R0)+
MOV CMPCPN,(R0)+
MOV FRCJTS,(R0)+
MOV R0,FDATA
.ENDC
BIC #BACKG,FSTAT ;BACKGROUND JOB DONE
DISMIS
.IFF ;THIS MATCHES THE ".IFZ DIAGY" AT THE START OF THE FORCE RTNS.
FRCLV7:
.ENDC
;END OF "FBACK"
;DEFINITIONS FOR PROGRAMS USING WRIST SENSOR
;BITS INDICATING WHICH FORCES ARE TO BE GATHERED ARE DEFINED AS FOLLOWS:
XFBIT == 1
YFBIT == 2
ZFBIT == 4
XMBIT == 10
YMBIT == 20
ZMBIT == 40
TBLBIT == 10000 ;CONVERT READINGS TO TABLE COORDS (DEFAULT IS HAND COORDS)
;BITS INDICATING WHICH JOINTS TO GATHER RESOLVED TORQUES FOR ARE:
T1BIT == 100 ;JOINT 1
T2BIT == 200 ;2
T3BIT == 400 ;3
T4BIT == 1000 ;4
T5BIT == 2000 ;5
T6BIT == 4000 ;6
;SETBAS - PROCEDURE TO SET THE BASE READINGS FOR THE FORCE WRIST
;THIS PROCEDURE IS USED TO TAKE AN INITIAL SET OF READINGS TO BE
;USED AS AN OFFSET IN RESOLVING THE SCHEINMAN FORCE SENSING WRIST.
;THE ONLY ARGUMENT REQUIRED BY THIS ROUTINE IS A POINTER
;TO A INTEGER ARRAY 8 WORDS LONG. AFTER EXECUTION, THE STRAIN
;GAGE READINGS ARE RETURNED IN THIS ARRAY AS WELL AS BEING LEFT
;IN AN INTERNAL ARRAY WHICH IS USED BY THE FORCE RESOLVING
;ROUTINE "WRIST". IF YOU DON'T NEED THE STRAIN GAGE READINGS,
;THE REGISTER WHICH NORMALLY CONTAINS THE ARRAY POINTER SHOULD BE
;SET TO ZERO. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #SAVEBASE,R0 ;SET TO 0 IF NOT NEEDED
; JSR PC,SETBAS
;
;NO ERROR MESSAGES ARE RETURNED BY THIS ROUTINE.
;REGISTERS USED:
; R0 PASSES AN ARGUMENT AND IS NOT ALTERED
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
SETBAS: INC BASLCK ;CHECK IF LOCKED OUT
BEQ 1$ ;BRANCH IF FREE TO GO ON
DEC BASLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR SETBAS
1$: MOV R0,BASPTR ;SET POINTER TO RETURN BASE VALUES
BNE .+10 ;DONT RETURN VALUES?
MOV #BASRDG,BASPTR
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),BASEVT ;SAVE EVENT NUMBER FOR LEVEL 7 CODE
SCHED7 #BASPPB,#1
EVWAIT (SP) ;WAIT FOR THE LEVEL 7 JOB TO FINISH
EVKIL
DEC BASLCK ;RELEASE CODE FOR FUTURE USE
RTS PC ;RETURN
;LEVEL 7 CODE FOR "SETBAS"
BASLV7: MOV AC,-(SP) ;SAVE REGS.
MOV BC,-(SP)
MOV CC,-(SP)
MOV DC,-(SP)
MOV EC,-(SP)
MOV DAT,-(SP) ;SAVE POINTER TO BASRDG
MOV #8.,DC
2$: CLR (DAT)+ ;CLEAR BASRDG FOR ACUMULATING READINGS
SOB DC,2$
MOV #4,DC
3$: MOV #30,BC ;GET FIRST WRIST CHANNEL ADDRESS
MOVB BC,ADCCHN ;START FIRST CONVERSION
MOV #10,CC ;SET COUNTER TO READ 8. CHANNELS
MOV (SP),DAT ;GET FIRST BASRDG ADDRESS
1$: TSTB ADCSR ;WAIT FOR CONVERSION
BPL .-4
ADD ADCVAL,(DAT)+ ;SAVE NEW READING
TSTB (BC)+ ;POINT TO NEXT CHANNEL
MOVB BC,ADCCHN ;START CONVERTING NEXT CHANNEL
SOB CC,1$ ;REPEAT UNTIL DONE 8 SENSORS
SOB DC,3$ ;REPEAT READINGS 4 TIMES
MOV BASPTR,DC ;GET PTR TO USER STORAGE ARRAY
MOV (SP)+,DAT
MOV #8.,AC
4$: ASR (DAT) ;DIVIDE BY 4
ASR (DAT)
MOV (DAT)+,(DC)+ ;SAVE DATA FOR USER
SOB AC,4$
MOV (SP)+,EC ;RESTORE REGS.
MOV (SP)+,DC
MOV (SP)+,CC
MOV (SP)+,BC
MOV (SP)+,AC
5$: EVSIG BASEVT ;SIGNAL END OF LEVEL 7 CODE
DISMS7 ;GO KILL THE JOB
;LOCAL STORAGE AREA
DATA
BASEVT: .BLKW 1 ;LEVEL 7 CODE COMPLETION EVENT
BASPTR: .BLKW 1 ;PTR TO ARRAY TO STORE READINGS IN FOR USER
BASRDG: .BLKW 8. ;ARRAY TO STORE READINGS FOR "WRIST"
BASPPB: BASPDB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0
BASPDB: PDBLK7 BASLV7,BASRDG,20
BASLCK: -1 ;-1 → SETBAS READY FOR USE, 0 → LOCKED UP
CODE
;END OF "SETBAS"
;WRIST - PROCEDURE TO READ AND RESOLVE THE FORCE WRIST READINGS
;THIS PROCEDURE IS USED TO READ AND RESOLVE THE FORCE WRIST STRAIN
;GAGE READINGS INTO EQUIVALENT FORCE AND MOMENT COMPONENTS. THE
;ARGUMENTS REQUIRED BY THIS ROUTINE ARE A POINTER TO A 6x8 CALIBRATION
;TABLE USED TO CONVERT THE READINGS TO FORCES/MOMENTS AND A POINTER
;TO A 1x6 ARRAY INTO WHICH THE COMPUTE FORCE/MOMENT COMPONENTS ARE STORED.
;IF THE STANDARD CALIBRATION MATRIX TO RESOLVE FORCES AND MOMENTS
;IN THE REFERENCE FRAME OF THE FORCE WRIST IS DESIRED, THE POINTER
;TO THE 6x8 CAN BE SET TO ZERO. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #CALTAB,R0 ;SET TO 0 IF NOT NEEDED
; MOV #FORCES,R1 ;PTR TO TABLE OF RESULTS
; JSR PC,WRIST
;
;NO ERROR MESSAGES ARE RETURNED BY THIS ROUTINE.
;REGISTERS USED:
; R0,R1 PASS ARGUMENTS AND ARE NOT ALTERED
;THE FOLLOWING CODE IS NON-REENTRANT, WAIT TILL FREE TO USE
WRIST: INC WSTLCK ;CHECK IF LOCKED OUT
BEQ 1$ ;BRANCH IF FREE TO GO ON
DEC WSTLCK ;ELSE WAIT AND TRY AGAIN
SLEEP #5
BR WRIST
1$: MOV R0,CALPTR ;SET POINTER TO CALIBRATION DATA
BNE .+10
MOV #STDCAL,CALPTR ;USE STANDARD CALIBRATION AS DEFAULT
MOV R1,FORPTR ;STORE COMPUTED F/M'S IN HERE
EVMAK ;CREATE A EVENT TO WAIT ON
MOV (SP),WSTEVT ;SAVE EVENT NUMBER FOR LEVEL 7 CODE
SCHED7 #WSTPPB,#1
EVWAIT (SP) ;WAIT FOR THE LEVEL 7 JOB TO FINISH
EVKIL
DEC WSTLCK ;RELEASE CODE FOR FUTURE USE
RTS PC ;RETURN
;LEVEL 7 CODE FOR "WRIST"
WSTLV7: MOV AC,-(SP) ;SAVE REGS.
MOV BC,-(SP)
MOV CC,-(SP)
MOV DC,-(SP)
MOV EC,-(SP)
MOVB #30,ADCCHN ;START ADC CONVERTING FIRST CHANNEL
MOV FORPTR,BC ;INITIALIZE ARRAY TO SAVE DATA IN
MOV #6,AC ;ZERO ALL 6 ELEMENTS
CLRF (BC)+ ;*K COULD PUT XFORMED BASE READINGS HERE
SOB AC,.-2
MOV #31,BC ;POINT TO NEXT WRIST CHANNEL
MOV #10,WSTCNT ;SET COUNTER TO READ 8. CHANNELS
MOV #BASRDG,CC ;POINTER TO BASE SENSOR READINGS
MOV (DAT),DAT ;POINT TO CALIBRATION TABLE
1$: TSTB ADCSR ;WAIT FOR CONVERSION
BPL 1$
MOV ADCVAL,AC
MOVB BC,ADCCHN ;START CONVERTING NEXT CHANNEL
TSTB (BC)+ ;POINT TO NEXT WRIST CHANNEL
SUB (CC)+,AC ;SUBTRACT BASE READING FROM SENSOR READING
LDCIF AC,AC1 ;SAVE CORRECTED READING
MOV FORPTR,EC ;SAVE SUMS OF PRODUCTS IN HERE
MOV #6,DC ;SIX SUMS IN ALL, ONE FOR EACH FORCE/MOMENT
2$: LDF (DAT)+,AC0 ;MULT ADC READING TIMES CALIBRATION MATRIX
MULF AC1,AC0
ADDF (EC),AC0 ;ACCUMULATE FORCE COMPONENTS
STF AC0,(EC)+
SOB DC,2$ ;REPEAT FOR ALL SIX FORCES/MOMENTS
DEC WSTCNT ;REPEAT FOR ALL EIGHT STRAIN GAGES
BGT 1$
MOV (SP)+,EC ;RESTORE REGS.
MOV (SP)+,DC
MOV (SP)+,CC
MOV (SP)+,BC
MOV (SP)+,AC
;RTS PC
EVSIG WSTEVT ;SIGNAL END OF LEVEL 7 CODE
DISMS7 ;GO KILL THE JOB
;LOCAL STORAGE AREA
DATA
CALPTR: .BLKW 1 ;PTR TO CALIBRATION TABLE
FORPTR: .BLKW 1 ;PTR TO ARRAY TO SAVE RESULTS IN
WSTCNT: .BLKW 1 ;LOOP COUNTER
WSTLCK: -1 ;-1 → WRIST READY FOR USE, 0 → LOCKED UP
WSTEVT: .BLKW 1 ;LEVEL 7 CODE COMPLETION EVENT
;PUT CALIBRATION DATA AT FRONT OF ARM DATA IN AL VERSION OF CODE
.IFZ STDALN
YXX == .
. = ARMDAT
.ENDC
STDCAL: ;STANDARD CALIBRATION MATRIX TRANSPOSE (ENGLISH)
.FLT2 -.2325081E-2 , -.3083945E-2 , .4604810E-1 , -.5110909E-1 , -.3235031E-2 , .2641738E-2
.FLT2 .9794261E-1 , -.4324512E-2 , -.2915139E-2 , .3185859E-1 , .5522090E-1 , -.3471358E-1
.FLT2 .7339207E-3 , .1606639E-2 , .6804592E-1 , .7163971E-2 , -.4028634E-1 , .5120467E-2
.FLT2 .9573883E-2 , .7935277E-1 , -.5759057E-2 , -.2655063E-1 , .6458532E-2 , -.4813714E-1
.FLT2 -.5962288E-2 , -.1015981E-2 , -.5389340E-1 , -.5217535E-1 , .9762089E-4 , -.3138004E-2
.FLT2 .1011939 , -.1167195E-1 , .2421104E-2 , .3971796E-1 , .5830861E-1 , .4213376E-1
.FLT2 .7793080E-2 , -.7565245E-2 , -.5839136E-1 , .5192415E-2 , -.3359392E-1 , -.2440074E-2
.FLT2 -.9898057E-2 , .7865053E-1 , .3517145E-2 , -.2578878E-1 , -.5119964E-2 , .4037705E-1
.IFZ STDALN
. = YXX
.ENDC
WSTPPB: WSTPDB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0
WSTPDB: PDBLK7 WSTLV7,CALPTR,20
CODE
;END OF "WRIST"
;GATHER - PROCEDURE TO COLLECT FORCE DATA
CODE
;THIS PROCEDURE INITALIZES THE DATA GATHERING SYSTEM. THE FORCE WRIST IS
;READ AT 30 Hz AND SELECTED FORCES ARE STORED IN AN ARRAY. BITS IN ANY
;MEANINGFUL COMBINATION IN R0 DETERMINE WHICH FORCES ARE TO BE RECORDED
;ACCORDING TO THE DEFINITIONS ON PAGE 53 ( ONLY HAND COORDS ARE VALID NOW ).
;R1 SHOULD CONTAIN THE ADDRESS OF A POINTER TO AN AREA FOR FLOATING POINT
;STORAGE AND R2 SHOULD CONTAIN THE ADDRESS OF A POINTER TO AN AREA FOR INTEGER
;STORAGE. IF R1 OR R2 IS ZERO DEFAULT AREAS WILL BE USED. R3 CONTAINS
;AN IDENTIFICATION NUMBER TO BE PASSED BACK AT BEGINNING OF INTEGER BUF.
;
;THIS ROUTINE SHOULD BE CALLED JUST PRIOR TO CALLING THE MOVE FOR WHICH
;DATA IS TO BE COLLECTED:
;
; MOV #FBITS,R0
; MOV #FPPTR,R1
; MOV #IPTR ,R2
: MOV #CONTROL,R3
; JSR PC,GATHER
;
;R0,R1 AND R2 PASS ARGUMENTS AND ARE CHHANGED
GATHER:
TST R1 ;IF R1|R2 =0 USE DEFAULT AREAS
BEQ 1$
TST R2
BNE 2$
1$: MOV #FPPTR,R1 ;POINT TO FP POINTER
MOV #GAREA,FPBUF
MOV #GAREA,FPPTR
MOV #INTPTR,R2 ;POINT TO INT POINTER
MOV #GIAREA,INTBUF
MOV #GIAREA,INTPTR
2$: TST R0
BEQ 5$ ;QUIT IF NO CONTROL BITS
MOV R0,GDATA ;SAVE CONTROL BITS
MOV R1,FPSAV ;SAVE POINTER TO FP POINTER
MOV R2,ISAV ;SAVE POINTER TO INTEGER POINTER
MOV R3,IDSAV ;SAVE ID# *K
CLR GTIME ;ZERO TIMES COUNTER
CLR R0 ;ZERO WRIST READINGS
mov #bluarm,c1 ;indicate blue arm using force system
5$: RTS PC
COMMENT ⊗
;LEVEL 7 JOB TO GATHER DATA
;AT START DAT←#EPSF
GATLV7: ;STF AC0,-(SP)
;STF AC1,-(SP)
TST GDATA,R0 ;CHECK CONTROL WORD
BEQ GATFIN ;QUIT IF CLEARED
RESCH7 #16. ;ELSE RESCHEDULE JOB
TST (SP)+ ;POP SCHEDULE TIME OFF STACK
JSR PC,RNS ;RESOLVE AND STORE FORCE READINGS
GETEPS: MOVB #30,ADCCHN ;START FIRST CONVERSION
MOV #BASRDG,AC ;POINT TO BASE READINGS
MOV #8.,BC ;EIGHT SENSORS TO READ
MOV #31,DC ;NEXT CHANNEL TO READ
MOV #EPS,EC ;INTEGER READING ARRAY
1$: TSTB ADCSR
BPL 1$
MOV ADCVAL,CC ;GET READING
MOVB DC,ADCCHN ;START NEXT READING
TSTB (DC)+ ;POINT TO NEXT CHANNEL
SUB (AC)+,CC ;SUBTRACT BASE READING
;FILTERING ON CC GOES HERE
MOV CC,(EC)+ ;ADD CHANGE TO EPS ARRAY
LDCIF CC,AC0
STF AC0,(DAT)+ ;STORE FLOATING POINT EPS
DEC BC
BGT 1$
GATFIN: DISMS7 ;DONE
⊗
;SUBROUTINE TO RESOLVE AND STORE READINGS AS NEEDED
RNS: INC GTIME ;UPDATE COUNTER
MOV GPTR,R1 ;GET DATA POINTER
MOV GDATA,R0 ;GET CONTROL BITS redundant
TST R0
BPL 1$
RTS PC ;QUIT IF NO RESOLVING NEEDED
;GET FORCE READINGS IN HAND COORDS
1$: BIC #177700,R0 ;CLEAR LEFT BYTE OF TORQUE BITS
MOV #STDCAL,R2 ;POINT TO CALIBRATION MATRIX FOR FORCES
GCK: BIT #1,R0 ;STORE THIS READING?
BNE 2$
ADD #4,R2 ;NO, ADVANCE CAL POINTER TO NEXT COL
BR 3$
2$: CLRF AC1 ;COMPUTE THIS FORCE COMPONENT
MOV #EPSF,R4 ;POINT TO GAGE READINGS
MOV #8.,R3 ;8 *'S
1$: LDF (R2),AC0 ;GET CAL ENTRY
MULF (R4)+,AC0 ;* EPSF
ADD #6*4,R2 ;POINT TO NEXT ITEM IN COL.
ADDF AC0,AC1 ;ACCUMULATE
DEC R3
BGT 1$
STF AC1,(R1)+ ;STORE FORCE READING IN DATA AREA
ADD #4-<8.*6*4>,R2 ;POINT TO NEXT COL
3$: ASR R0 ;CHECK NEXT CONTROL BIT
BNE GCK ;CONTINUE UNTIL NO MORE BTS
;CHECK FOR JOINT TORQUE GATHERING
MOV GDATA,R0 ;GET LEFT BYTE
BIC #170077,R0 ;KEEP ONLY TORQUE BITS
ASH #-6,R0 ;RIGHT JUSTIFY BITS
MOV #ERRPTR,R2 ;POINT TO LIST OF TORQUE ERROR POINTERS
ADD #14.,R2 ;POINT TO BLUE ARM TERMS
4$: BIT #1,R0 ;COLLECT THIS COMPONENT?
BEQ 5$
LDF @(R2),AC0 ;GET ERROR VALUE
STF AC0,(R1)+ ;AND STORE IT
5$: ADD #2,R2 ;NEXT ERROR TERM
ASR R0
BNE 4$
MOV R1,GPTR ;SAVE DATA POINTER
RTS PC
;GATHER DATA AREA
DATA
;GATPPB: GATPBB ;ARRAY TO INITALIZE LEVEL 7 CODE
; .WORD 0,0 ;TIME AND END OF LIST
;GATPBB: PDBLK7 GATLV7,EPSF,20
gcnt: .word 0 ;transient gather timer
GDATA: .WORD 0 ;CONTAINS BITS INDICATING COMPONENTS TO GATHER
GTIME: .WORD 0 ;INDICATES WHICH PASS WE ARE ON
GPTR: .WORD 0 ;POINTS TO NEXT FP STORAGE LOCATION removed by MSM
FPSAV: .WORD 0
GIPTR: .WORD 0 ;POINTS TO INTEGER STORAGE LOCATION
ISAV: .WORD 0
IDSAV: .WORD 0 ;STORAGE FOR ID#
EPS: .BLKW 8. ;STORAGE FOR STRAIN GAGE READINGS
EPSF: .BLKW 2*8. ;FP STORAGE FOR READINGS
;END OF GATHER
;FORCE SENSING ROUTINES DATA AREA
DATA
FZAPS: ;START OF AREA ZEROED BY "INTARM"
FSTAT: .WORD 0 ;FORCE SENSING SYSTEM STATUS WORD, CONTAINS:
;NXTFIN ==1 ;NEXT SERVICING PERIOD WILL BE "FINAL".
;FINAL ==2 ;IN FINAL STAGE OF MOTION, JUST NULLING ERRORS
;RUN ==4 ;LEVEL 7 CODE SCHEDULED TO EXECUTE
PASSGO ==10 ;LEVEL 5 CODE INITIALIZED AND READY TO GO
BACKG ==20 ;BACKGROUND JOB RUNNING AT LEVEL 5
;FIRST ==40 ;FIRST PASS THRU FORCE CODE
; 177400 ;≠0 MEANS LEVEL 7 CODE LOCKED OUT
CMPCPN: .BYTE 0 ;COMPLIANT COMPONENTS 1→FX, 40→MZ
NEWCPN: .BYTE 0 ;NEW COMPONENTS
FRCJTS: .BYTE 0 ;FORCE SERVOED ARM JTS 1→JT 1, 40→JT 6
NEWJTS: .BYTE 0 ;NEW FORCE SERVOED JTS
FRCNUM: .WORD 0 ;NUMBER OF JOBS PENDING
BCKTME: .WORD 3 ;=0 WHEN TIME TO RUN BACKGROUND JOB
XFPTR: .WORD 0 ;LINK TO JOB BLKS
YFPTR: .WORD 0
ZFPTR: .WORD 0
XMPTR: .WORD 0
YMPTR: .WORD 0
ZMPTR: .WORD 0
BIASF: .FLT2 0.0 ;STORAGE FOR BIAS FORCES
.FLT2 0.0 ;need to be zeroed by intarm or.. *k
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
BIAST: .FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
.FLT2 0.0
FZAPE: ;END OF ZERO AREA
FCARAY: C1 ;PTR TO CURRENT C ARRAY AND ARM DATA
C1: 0 ;YELARM/BLUARM AND TABLE/HAND BITS
.BLKW 12.*2 ;FORCE TRANS
C2
C2: 0
.BLKW 12.*2
C1
NEXTC== C2-C1-2 ;REL. PTR TO RING LINKAGE WORD
FJARAY: J1 ;PTR TO CURRENT JACOBIAN ARRAY
J1: .BLKW 42.*2 ;JACOBIAN ARRAY + SUM OF SQUARES
J2
J2: .BLKW 42.*2
J1
NEXTJ== J2-J1-2 ;REL. PTR TO RING LINKAGE WORD
DELTA: .FLT2 1.0,0.0,0.0 ;DIFFERENTIAL CHANGE TRANSFORMATION
.FLT2 0.0,1.0,0.0
.FLT2 0.0,0.0,1.0
.FLT2 0.0,0.0,0.0
DELPTR: DELTA+<9.*4> ;DEL X
DELTA+<10.*4> ;DEL Y
DELTA+<11.*4> ;DEL Z
DELTA+<5.*4> ;DEL THX
DELTA+<6.*4> ;DEL THY
DELTA+<1.*4> ;DEL THZ
IMAT: .FLT2 1.0,0.0,0.0 ;IDENTITY MATRIX
.FLT2 0.0,1.0,0.0
.FLT2 0.0,0.0,1.0
.FLT2 0.0,0.0,0.0
TTRAN: .BLKW 12.*2 ;TEMPORARY TRANSFORM STORAGE
CTC: .BLKW 12.*2 ;C_T_0 (P+1)* 0_T_C(P+2)
OT6N:
TCP2: .BLKW 12.*2 ;0_T_C POLY AT TIME P+2
OTCP: .BLKW 12.*2 ;0_T_C CORRECTED FOR TIME P+1
OTON:
OTCN: .BLKW 12.*2 ;0_T_C CORRECTED FOR TIME P+2
OP6: .BLKW 3*2 ;OLD POLYNOMIAL POSITION
CMPFRC: .BLKW 6*2 ;FORCE COMPLIANCE LEVELS
FVALUE: .BLKW 6*2 ;JOINT FORCE COMPLIANCE LEVELS
TCOR: .BLKW 12.*2 ;CORRECT PREDICTED TRANSFORMATION
TTHP: TTH-14.
TTH: .WORD .+14,.+16,.+20 ;TABLE OF PTRS TO STORAGE FOR JOINT ANGLES
.WORD .+22,.+24,.+26
TTH1: .BLKW 6*2
TTH7:
TTHCP: TTHC-14.
TTHC: .WORD .+14,.+16,.+20 ;TABLE OF PTRS TO STORAGE FOR JOINT ANGLES
.WORD .+22,.+24,.+26
TTHC1: .BLKW 6*2
TRANSP: .WORD 12.,12.,-20.,12.,12.,-20.,12.,12.,4
FSCALE: 0 ;JACOBIAN SCALE FACTORS , *1
0 ;*1
1200 ;*32
600 ;*8
600 ;*8
1000 ;*16
FDATPT: .BLKW 1 ;POLYNOMIAL DATA INFORMATION, DATPT PTR
FPOLY: .BLKW 1 ;POLY PTR OF JT 6 + 4
FINUSE: .BLKW 1 ;INUSE
FPTIME: .BLKW 1 ;PTIME
FTTME: .BLKW 1 ;TTIME
FFTIME: .BLKW 2 ;FTTIME
FDTH: .BLKW 6*2 ;CHANGE IN FORCE SERVOED JOINTS ANGLES
SRVPTR: .BLKW 1 ;PTRS TO ARM JTS
FRCPTR: FRCJOB ;PTR TO CHAIN OF FREE BLKS IN JOB LIST
FRCJOB: .BLKW 7*FRCBLK;JOB BLOCKS CONTAIN:
; WRD 1: LINK TO NEXT BLOCK
; WRD 2,3: F.P. MAGNITUDE
; WRD 4: FORCE OPTION BITS
; WRD 5: COUNTER FOR # OF TIMES TEST OK
; WRD 6,7: STRT ADDR OF JOB&PDB
FRCPPB: FRCPBB ;ARRAY TO INITIALIZE LEVEL 7 CODE
.WORD 0,0 ;TIME AND END OF LIST
FRCPBB: PDBLK7 FRCLV7,XFPTR,20
FRCPD5: PDBLK 5,100,FP;LEVEL 5 PDB
;END OF "FORCE" ROUTINES
;SETSTF ROUTINE TO INITALIZE STIFFNESS SYSTEM
;R0 SHOULD POINT TO LIST OF 6 FP STIFFNESS VALUES: Kx,Ky,Kz,Gx,Gy,Gz
;R1 SHOULD POINT TO A RELATIVE TRANSFORM BETWEEN THE HAND FRAME OF
;REFERENCE AND THE FRAME IN WHICH THE STIFFNESS VALUES ARE DEFINED
;
;STFDAT IS INITALIZED
;R0 GARBAGED, ALL OTHER REGS PRESEVED
CODE
SETSTF: MOV R1,-(SP)
MOV R2,-(SP)
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
;PUT REL XFORM TO STIFFNESS FRAME IN C66
MOV R1,RELX ;SAVE POINTER TO REL. XFORM
mov #c66,r2
mov #3,r3 ;3 cols in rel xform
3$: mov #3,r4 ;3 elements in each col
2$: ldf (r1)+,ac0 ;get element of orientation
stf ac0,(r2)+ ;store in upper left 3x3
stf ac0,<20.*4>(r2) ; " lower right
sob r4,2$
add #<3*4>,r2
sob r3,3$
MOV #C66,R2 ;transfer displacements
LDF (R1)+,AC0
STF AC0,<31.*4>(R2) ;X
NEGF AC0
STF AC0,<26.*4>(R2) ;-X
LDF (R1)+,AC0
STF AC0,<20.*4>(R2) ;Y
NEGF AC0
STF AC0,<30.*4>(R2) ;-Y
LDF (R1),AC0
STF AC0,<24.*4>(R2) ;Z
NEGF AC0
STF AC0,<19.*4>(R2) ;-Z
MOV #KC,R1 ;STORE CARTESIAN STIFFNESS VALUES HERE
MOV #6,R2
1$: LDF (R0)+,AC0
CFCC
BLT .+4
NEGF AC0 ;MAKE SURE ONLY NEG. STIFFNESS VALUES USED
STF AC0,(R1)+
SOB R2,1$ ;6 FP #'S TO TRANSFER
MOV #BLUARM,C1 ;INDICATE BLUE ARM USING FORCE SYSTEM *K
MOV #RUN,STFDAT ;INDICATE NEXT MOVE IS A KMOVE
;INITALIZE CI AND CII TERMS
MOV #BTH,R0 ;ELSE COMPUTE CURRENT DYNAMIC COEF
MOV #BCI,R1 ;UPDATE SERVO DATA BLOCKS
MOV #BLUARM,R2 ;INDICATE BLUE ARM
JSR PC,DTERMS ;COMPUTE NEW CI'S AND CII'S
;COMPUTE JACOBIAN MATRIX, STIFFNESS MATRIX AND JTCAL MATRIX
GETJAC: MOV #IMAT,R0 ;POINTER TO FORCE TRANS
MOV #THPTR,R1 ;POINT TO LIST OF ALL JOINT ANGLES
MOV #BLUARM+FHAND,R2;INDICATE BLUE ARM IN HAND COORDS
MOV #JC,R3 ;POINT TO RESULT MATRIX
JSR PC,JACOB ;COMPUTE JACOBIAN
JSR PC,GJTCAL ;COMPUTE JOINT TORQUE CALIBRATION MAT.
MOV RELX,R1 ;POINT TO REL TRANSFORM
JSR PC,GETKTH ;COMPUTE STIFFNESS MATRIX
MOV (SP)+,R5
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
MOV (SP)+,R1
RTS PC
;END OF SETSTF
;KMOVE MOVES ALONG TRAJECTORY WITH STIFFNESS K
;ASSUMES KTH,JTCAL ETC ARE READY
;CALLED FROM MOVE AFTER POLYNOMIALS ARE INITALIZED
KMOVE: CLR KPTIME ;ZERO TICKS INTO SEGMENT
MOV R0,r3 ;SAVE MOVE BLOCK POINTER
MOV R1,BLKPTR ;POINT TO DEVICE BLOCK
MOV DATST+SEGTME(R0),R2 ;GET FIRST SEGTIME
ASH #-4,R2 ;CONVERT TO SERVO PERIODS
LDCIF R2,AC0
STF AC0,KFTTIM ;STORE FP SEGTIM/16
;START DAC REFRESH ROUTINE
MOVB #10,174004 ;RESET BLUE ARM INTERFACE
BIT #6,174004 ;MOTOR POWER ON?
BEQ 2$ ;IF ON GO RUN
MOV #NOPOWR,R0 ;ELSE SIGNAL ERROR
BR KDONE
2$: MOVB #20,DACCSR ;RESET HARDWARE TIME-OUT
INCB KICDAC ;NEED TO SCHEDULE DAC REFRESH JOB?
BIT #KICRUN,KICDAC
BNE KSCHED ;NO, STILL RUNNING
CLR BPANIC
SCHEDU #MUGDAC,#DACSER,#USRDM,#20
BIS #KICRUN,KICDAC
KSCHED: MOV #WRGNUM,R0 ;ASSUME WRONG NUMBER OF JOINTS
MOVB (R1),R2 ;GET # OF SERVOS ATTACHED
BLE KDONE
CMP (R1)+,(R1)+ ;POINT TO FIRST SERVO
3$: MOV (R1)+,DAT
BIS #RUN+MOVING,(DAT)
JSR PC,BRKREL
SOB R2,3$
CLR R0
CLR KPASS ;INDICATE FIRST PASS
EVMAK
MOV (SP),KSVEVT ;SAVE EVENT
MOV #SCHBLK,R1
; TST GDATA
; BNE 10$
; TST FRCNUM
; BEQ 11$
;INITALIZE FPOLY
10$: MOV (r3),R3 ;GET SERVO BITs
MOV #BARM,R0 ;ASSUME BLUE ARM
MOV #SRV13,R2
BIT #BLUARM,@FCARAY ;WERE WE RIGHT?
BNE .+12
MOV #YARM,R0 ;NO
MOV #SRV06,R2
BIC R3,R0 ;MAKE SURE ALL 6 JTS ASSIGNED
BEQ 2$ ;OK
MOV #FWRGJT,R0 ;SIGNAL ERROR
JMP kdone
2$: MOV DATPT(R2),FDATPT;SAVE POLYNOMIAL DATA FOR FORCE RTNS
BNE 4$
MOV #NOPOLY,R0 ;SIGNAL CAN'T FORCE SERVO WITHOUT POLY
jmp kdone
4$: MOV POLY(R2),FPOLY
ADD #4,FPOLY
MOV INUSE(R2),FINUSE
MOV TTIME(R2),FTTME
MOV #FRCPBB,(R1)+ ;SCHEDULE JOB
TST (R1)+
11$: MOV #KSVPDB,(R1)+
TST (R1)+
CLR (R1) ;MARK END OF LIST
SCHED7 #SCHBLK,#10. ;SCHEDULE ALL LEVEL7 JOBS
EVWAIT (SP)
EVKIL
TST BPANIC
BEQ KDONE
MOV #NOPOWR,R0
KDONE: MOV R0,R3 ;SAVE ERROR #
MOV BLKPTR,R1 ;POINTER TO DEVICE BLOCK
MOVB (R1),R2 ;GET # ATTACHED
CMP (R1)+,(R1)+
1$: MOV (R1)+,DAT ;POINT TO SERVO
JSR PC,MOTSTP ;SHUT MOTOR DOWN
BIC #RUN+MOVING,(DAT)
SOB R2,1$
MOV R3,R0 ;ERROR #
DEC KICDAC ;TURN OFF DAC REFRESH JOB
CLR STFDAT ;DISABLE STIFFNESS SYSTEM
RTS PC
;KSRVO ROUTINE TO SERVICE ALL JOINTS ONCE PER TICK
;SERVOS EACH JOINT IN DEVICE BLOCK POINTED TO BY DAT
KSRVO: MOV BLKPTR,DAT
; MOV (DAT),DAT ;SHOULD POINT AT LIST OF SERVOS
TST (DAT) ;CHECK IF CATASTROPHIC ERROR
; BLE KQUIT ;out of range branch made into jump
bgt 4$
jmp kquit
4$: BIT #RUN,STFDAT
BEQ KQUIT ;STIFFNESS SRVRO IS OFF
BIT #1,BPANIC ;PANIC?
BNE KQUIT
TST KPASS ;FIRST PASS FOR THIS PERIOD?
BNE EVLP ;NOPE
INC KPASS ;YEP
CMP (DAT)+,(DAT)+ ;POINT TO FIRST SERVO POINTER
MOV DAT,KSRV ;SAVE SERVO POINTER
JSR PC,REPS ;READ GAGES FOR THIS PASS
LDCIF KPTIME,AC1 ;CONVERT THE PRESENT TIME TO FLOATING POINT
INC KPTIME ;INCREMENT FOR NEXT TIME
STF AC1,KFPTIM ;SAVE FOR EVLP
CMPF KFTTIM,AC1
CFCC
BGT EVLP ;GO SERVO IF NOT THRU SEGMENT YET
MOV KSRV,R2 ;PREPARE TO SWITCH SEG
MOV (R2)+,DAT
MOV DATPT(DAT),R0 ;PTR TO START OF POLY. DATA LIST
MOV RNCODE(R0),R1 ;CHECK IF POSSIBLE EVENT TO SIGNAL
BEQ 3$
TST 2(R1) ;GET EVENT #
BEQ 3$ ;ZERO IF ALREADY SIGNALED
MOV R0,-(SP) ;SAVE REGS SO KERNEL WON'T CLOBBER 'EM
MOV R1,-(SP)
MOV R2,-(SP)
MOV DAT,-(SP)
EVSIG 2(R1)
MOV (SP)+,DAT ;RESTORE REGS.
MOV (SP)+,R2
MOV (SP)+,R1
MOV (SP)+,R0
CLR 2(R1) ;ZERO TO INDICATE SIGNALED
3$: MOV (R0),R3 ;SWITCH TO A NEW SEGMENT
ADD R3,R0 ;MOVE PTR
TST (R0) ;FINAL SEGMENT?
BNE 1$ ;NO
BIC #RUN,STFDAT ;NORMAL END
BR KQUIT
1$: MOV SEGTME(R0),R1 ;GET NEW SEGTIME
ASH #-4,R1 ;CONVERT TO SERVO PERIODS
LDCIF R1,AC0
STF AC0,KFTTIM ;STORE NEW SEGTIM
2$: MOV R0,DATPT(DAT) ;RESET ALL JOINT POLY AND DATA POINTERS
ADD R3,POLY(DAT)
MOV (R2)+,DAT
BNE 2$
CLR KPTIME ;BEGINNING OF A NEW SEG.
CLRF KFPTIM
CLRF AC1 ;ZERO TIME
;SERVOING LOOP
EVLP: MOV @KSRV,DAT ;GET SERVO POINTER
BEQ KSVDN ;IF ZERO THEN DONE FOR THIS TICK
ADD #2,KSRV ;POINT TO NEXT SERVO POINTER
tst @ksrv
beq 2$
resch7 #2 ;schedule for next pass
tst (sp)+ ;pop time off
inc kpass
br 1$
2$: clr kpass ;read wrist next time
RESCH7 #6. ;RESCHEDULE IN 6 msec → 16 msec total
TST (SP)+ ;POP SCHED TIME OFF STACK
1$: LDF KFPTIM,AC1
MOV POLY(DAT),CC ;GET ABSOLUTE ADDRESS OF POSITION POLYNOMIAL
DIVF KFTTIM,AC1 ;GET THE FRACTIONAL TIME INTO THIS SEGMENT
LDF (CC),AC0 ;EVALUATE THE POSITION POLYNOMIAL, GET A5
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A4
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A3
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A2
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A1
MULF AC1,AC0 ; x T
ADDF -(CC),AC0 ; + A0, NOW HAVE POLYNOMIAL JOINT ANGLE
STF AC0,AC1 ;*K1 TRY THDP CHANGE HERE
SUBF THP(DAT),AC1 ;COMPUTE PREDICTED ANGULAR CHANGE
DIVF KELTIM,AC1 ;DIVIDE BY ELAPSED TIME
STF AC1,THDP(DAT) ;SAVE VEL.
STF AC0,THP(DAT) ;SAVE POSITION
JSR PC,SS2 ;SERVO JOINT POINTED TO BY DAT
disms7
bpt ;another strange place to be
; BR EVLP ;DO FOR ALL JOINTS IN DEVICE BLOCK
KQUIT: EVSIG KSVEVT ;SIGNAL SERVOING DONE WITH (OR CHG SEG)
KSVDN: ;JSR PC,RNS ;RESOLVE AND STORE FORCE/TORQUE READINGS FOR GATHER
DISMS7
; DISMISS
BPT ;BPT HERE IF FINISHED AFTER SCHEDULED
;PDB FOR STIFFNESS SERVO - LEVEL 7 -
DATA
KSVPDB: PDBLK7 KSRVO,BLKPTR,40 ;PROCESS DESCRIPTOR BLOCK
;SS2 COMPUTES TRAJECTORY DEVIATION AND DETERMINES FORCE TO APPLY
;EXECUTION TIME = .95 msec
;DAT POINTS TO SERVO TO SERVICE
CODE
SS2: JSR PC,ANGLES ;DETERMINE CURRENT POSITION AND VELOCITY
SUBF THP(DAT),AC0 ;COMPUTE DELTA THETA (POS ERROR)
MULF TOTQE(DAT),AC0 ;CONVERT TO RAD. OR IN.
STF AC0,POSERR(DAT) ;AND SAVE IT
;DETERMINE JOINT #
CLR R0
MOVB SRVNUM(DAT),R0
SUB #7,R0 ;COMPUTE JOINT NUMBER
MOV R0,-(SP) ;AND SAVE
;COMPUTE COMMANDED TORQUE FOR THIS JOINT
JSR PC,TCI ;COMPUTE TC FOR THIS JOINT
kn1: STF AC0,TC ;SAVE
;kn3: LDF KKV(DAT),AC1 ;COMPUTE VELOCITY FEEDBACK
; MULF CII(DAT),AC1 ;FOR STABILITY W/ INERTIAS
; DIVF KKK(DAT),AC1 ;FOR STABILITY W/ DIFFERENT STIFFNESS
; MULF THD(DAT),AC1 ;* ABSOLUTE VELOCITY
; ADDF AC1,AC0
;DETERMINE TORQUE ERROR, U, ON THIS JOINT
MOV (SP),R0 ;GET JOINT #
JSR PC,TSI ;GET TORQUE ON JOINT #R0
cmp #6,(sp) ;fix for inverted joint 6 torque
beq kn2
NEGF AC0
kn2: ADDF TC,AC0 ;TORQUE ERROR U=TC-TS
;COMPUTE TORQUE COMPENSATON
MOV (SP)+,R0 ;GET SERVO # AGAIN
JSR PC,SS1 ;U→AC0 AND SERVO # →R0 →→ Y→ACO
;ADD VELOCITY DAMPING AND GRAVITY COMPENSATON
kn3: LDF KKV(DAT),AC1
MULF CII(DAT),AC1 ;FOR STABILITY W/ INERTIAS
; MOV (SP)+,R0 ;GET SERVO # AGAIN
;jsr pc,gkkk
;divf ac2,ac1
DIVF KKK(DAT),AC1 ;FOR STABILITY W/ DIFFERENT STIFFNESS *fix me
ldf thd(dat),ac2 ;compute vel err in moving frame
subf thdp(dat),ac2 ;*K1 TRY THDP CHANGE HERE
mulf ac2,ac1 ;* relative velocity
; MULF THD(DAT),AC1 ;* ABSOLUTE VELOCITY
ADDF AC1,AC0
; addf tc,ac0 ;feed forward commanded torque *k1
ADDF CI(DAT),AC0 ;GRAVITY COMPENSATION
ldf v0(dat),ac1 ;get friction torque
tstf tc ;check sign of applied torque
cfcc
bge 1$ ;add friction torque in
negf ac1 ; direction of commanded torque
1$: addf ac1,ac0 ;
kn4: STF AC0,ERRTQE(DAT) ;SAVE FOR GATHER ROUTINES
JSR PC,MOTDRV ;DO IT
RTS PC
;SS1 COMPUTES FORCE COMPENSATION
;CALLED WITH AC0←U
; WHERE U=Tc-Ts
;RETURNS AC0←Y
; WHERE Y=(S+A)/(S+B) + Q
; Q= QOLD + Ti*SGN(U) FOR |U|>Tdbd
; = QOLD FOR |U|≥Tdbd
;COMPUTE Y1 (ZERO ORDER HOLD APPROX OF LEAD-LAG FILTER)
SS1: STF AC0,AC2 ;SAVE U FOR LATER
STF AC0,UOLD(DAT) ;SAVE U FOR NEXT PASS
LDF ALPHA,AC1
MULF UOLD,AC1
SUBF AC1,AC0
LDF BETA,AC1
MULF YOLD(DAT),AC1 ;BETA*Yold
ADDF AC1,AC0 ;Y1=U-ALPAH*Uold+BETA*yold
STF AC0,YOLD(DAT) ;SAVE FOR NEXT TIME
MULF KKF(DAT),AC0 ;GAIN
; ___
;COMPUTE Y2 (INTEGRAL W/ LIMITER AND DEADBAND)→ __| * 1/S
; ___|
LDF KKI(DAT),AC1 ;ASSUME NOT IN DEADBAND
TSTF AC2 ;WAS U POS OR NEG?
CFCC
BPL 2$
NEGF AC1 ;IF ERROR <0 NEGATE TI
2$: ABSF AC2
CMPF TDBD,AC2 ;TDBD<|U|?
CFCC
BLT 3$ ;BR IF TDBD < |U|
CLRF AC1 ;INSIDE DEADBAND SO 0 OUTPUT
3$: ADDF ERRINT(DAT),AC1 ;INTEGRATE FORCE ERROR
kn5: STF AC1,ERRINT(DAT) ;SAVE INTEGRAL
ADDF AC1,AC0 ;Y= KKF*Y1 + Y2
RTS PC
;REPS ROUTINE TO READ RAW FORCE SENSOR DATA
;READS FORCE SENSOR ON BLUARM AND SUBTRACTS BASE READINGS FROM IT.
;LEAVES FLOATING POINT CORRECTED READINGS AT EPSF.
;PRESERVES DAT AND ALTERS R0-R4,AC0
;RUNTIME IS .5 MSEC
REPS: MOV DAT,-(SP) ;SAVE DAT
MOV #EPSF,DAT ;WHERE TO PUT FLOATING PT EPS
MOVB #30,ADCCHN ;START FIRST CONVERSION
MOV #BASRDG,R0 ;POINT TO BASE READINGS
MOV #8.,R1 ;EIGHT SENSORS TO READ
MOV #31,R3 ;NEXT CHANNEL TO READ
MOV #EPS,R4 ;INTEGER READING ARRAY
1$: TSTB ADCSR
BPL 1$
MOV ADCVAL,R2 ;GET READING
MOVB R3,ADCCHN ;START NEXT READING
TSTB (R3)+ ;POINT TO NEXT CHANNEL
SUB (R0)+,R2 ;SUBTRACT BASE READING
;FILTERING ON CC GOES HERE
MOV R2,(R4)+ ;ADD CHANGE TO EPS ARRAY
LDCIF R2,AC0
STF AC0,(DAT)+ ;STORE FLOATING POINT EPS
DEC R1
BGT 1$
MOV (SP)+,DAT ;RESTORE DAT
RTS PC
;TCI,TSI ROUTINES TO COMPUTE COMMANDED AND SENSED TORQUE IN I-TH JOINT
;TCI PROCEDURE TO COMPUTE COMMANDED TORQUE FOR I-TH JOINT BASED ON STIFFNESS
;MATRIX, KTH. R0 SHOULD CONTAIN NUMBER OF JOINT TO COMPUTE TORQUE FOR.
;R0-R2 AND AC1 DESTROYED. AC0 CONTAINS RESULT.
TCI: DEC R0
BGE 1$
BPT
1$: MOV R0,-(SP) ;SAVE
MUL #24.,R0 ;COMPUTE OFFSET INTO KTH (ANS IN R1)
ADD #KTH,R1 ;POINT TO PROPER ROW OF KTH
MOV #PERPTR,R0 ;POINT TO LIST OF POINTERS TO POS. ERRS.
MOV #6,R2
CLRF AC0
2$: LDF @(R0)+,AC1 ;POSERR
MULF (R1)+,AC1 ;*K(I,J)
ADDF AC1,AC0 ;ACCUMULATE
SOB R2,2$ ;6 TIMES
MOV (SP)+,R0
MUL #4.,R0 ;COMPUTE OFFSET FOR BIAS TORQUE
ADDF BIAST(R1),AC0 ;ADD BIAS IF ANY
RTS PC
;TSI COMPUTES SENSED TORQUE ON I-TH JOINT OF BLUE ARM
;R0 SHOULD CONTAIN NUMBER OF JOINT FOR WHICH TORQUE IS DESIRED
;RESULT IS RETURNED IN AC0 (OZ-IN)
;DESTROYS R0-R2,AC0,AC1
TSI: DEC R0
BGE 1$
BPT
1$: ASH #5,R0 ;COMPUTE OFFSET IN JTCAL
ADD #JTCAL,R0 ;POINT TO PROPER ROW OF JTCAL
MOV #8.,R1
CLRF AC0
MOV #EPSF,R2
2$: LDF (R0)+,AC1
MULF (R2)+,AC1
ADDF AC1,AC0
SOB R1,2$
RTS PC
;GJTCAL,FINKTH ROUTINES TO COMPUTE TORQUE TRANSFORM AND STIFFNESS MATRICES
;ROUTINE COMPUTES MATRIX THAT TRANSFORMS FORCE SENSOR READINGS TO JOINT TORQUES
;DESTROYS R0-R5,AC0,AC1
;ASSUMES JC HAS BEEN COMPUTED ALREADY
CODE
GJTCAL: MOV #JC,R0 ;points to jacobian in hand coords.
MOV #STDCAL,R1
MOV #JTCAL,R2
MOV #6,R5 ;6 ROWS TO CALCULATE
1$: MOV #8.,R4 ;8 ELEMENTS IN EACH ROW
2$: MOV #6,R3 ;6 PRODUCTS TO MAKE AN ELEMENT
CLRF AC0
3$: LDF (R0)+,AC1 ;GET JT ELEMENT
MULF (R1)+,AC1 ;* CAL ELEMENT
ADDF AC1,AC0 ;ACCUMULATE
SOB R3,3$
STF AC0,(R2)+ ;STORE ELEMENT OF JTCAL
SUB #24.,R0 ;BACK TO PREVIOUS ROW OF JT
SOB R4,2$
ADD #24.,R0 ;NEXT ROW OF JT
SUB #192.,R1 ;BACK TO BEGINNING OF CAL
SOB R5,1$
RTS PC
;ROUTINE TO COMPUTE JOINT STIFFNESS MATRIX
;R1 SHOULD POINT TO TRANS BETWEEN HAND AND STIFFNESS FRAME
;
; CLOBBERS R0-R4
GETKTH: MOV R5,-(SP) ;SAVE R5
;COMPUTE JACOBIAN TO STIFFNESS FRAME
MOV #JPRIME,R0
MOV #C66,R1
MOV #JC,R2
MOV #6,R5 ;6 COLS TO FILL IN RESULT
JSR PC,MATM66
;COMPUTE INTERMEDIATE PRODUCT (KC*J')
MOV #JPRIME,R1 ;POINT TO NEW JACOBIAN
MOV #KCJ,R2 ;POINT TO STORAGE FOR KC*J MATRIX
MOV #6,R4 ;6 COLUMNS
2$: MOV #KC,R0 ;POINT TO CARTESIAN STIFFNESS MATRIX
MOV #6,R3 ;6 ENTRIES/COLUMN
1$: LDF (R1)+,AC0 ;GET ENTRY
MULF (R0)+,AC0 ;* KC
STF AC0,(R2)+ ;STORE RESULT
SOB R3,1$
SOB R4,2$
;COMPUTE J'T*(KC*J')=KTH
FINKTH: MOV #JPRIME,R0
MOV #KTH,R1 ;POINT TO MATRIX TO STORE KTH (COLUMN MAJOR)
MOV #KCJ,KCJCOL ;POINT TO FIRST COLUMN OF KCJ
MOV #6,R5 ;DO FOR 6 COLUMNS OF KCJ
3$: MOV #JPRIME,R0 ;POINT TO J'T (JPRIME TRANSPOSE)
MOV #6,R3 ;COMPUTE SIX ENTRIES FOR THIS COLUMN
1$: MOV KCJCOL,R2 ;POINT TO COLUMN OF KCJ
CLRF AC2 ;CLEAR ACCUMULATOR
MOV #6,R4 ;EACH KTH ENTRY COMES FROM 6 PRODUCTS
2$: LDF (R2)+,AC0 ;GET ITEM FROM COLUMN OF KCJ
LDF (R0)+,AC1 ;GET CORRESPONDING ROW ITEM FROM JT
MULF AC0,AC1
ADDF AC1,AC2 ;ACCUMULATE
SOB R4,2$ ;DO 6 OF 'EM
STF AC2,(R1)+ ;SAVE ENTRY IN KTH
SOB R3,1$ ;FOR ALL 6 ROWS OF JT
ADD #24.,KCJCOL ;POINT TO NEXT COLUMN OF KCJ
SOB R5,3$ ;AND DO AGAIN
MOV (SP)+,R5 ;RESTORE R5
RTS PC
;MATM66 MULTIPLIES 6*6 BY 6*1 VECTORS OR 6*6 MATRICES
;PERFORMS (R0)←(R1)*(R2)
;R0-R5 DESTROYED
;R5 SHOULD CONTAIN 1 IF FOR MATRIX VECTOR PRODUCT
;R5 SHOULD CONTAIN 6 IF FOR MATRIX MATRIX PRODUCT
;(R0) SHOULD NOT BE SAME AS (R1) OR (R2)
;2.5 MSEC EXECUTION TIME
MATM66: MOV R1,-(SP) ;SAVE ROW POINTER
11$: MOV (SP),R1 ;FIRST ROW AGAIN
MOV #6,R4 ;6 ROWS TO DO
BR 3$
6$: SUB #116.,R1 ;NEXT ROW
SUB #24.,R2 ;SAME COL
3$: MOV #6,R3 ;DOT ROW AND COL TOGETHER
CLRF AC0 ;CLEAR ACCUMULATOR
BR 2$
1$: ADD #24.,R1 ;NEXT ITEM IN ROW
2$: LDF (R2)+,AC1 ;GET NEXT ITEM IN COL.
CFCC
BEQ 27$ ;SKIP ZEROS IN SPARSE MATRIX
MULF (R1),AC1 ;* NEXT ITEM IN COL
ADDF AC1,AC0
27$: SOB R3,1$ ;6 ITEMS TO DO
STF AC0,(R0)+ ;STORE (I,J) RESULT
SOB R4,6$ ;6 ROWS TO DO
SOB R5,11$ ;6 COLS TO FILL
TST (SP)+ ;CLEAR STACK
RTS PC
;GKKK GETS PRINCIPAL STIFFNESS COEFFICENT FOR I'TH JOINT
;R0 SHOULD CONTAIN NUMBER OF JOINT FOR WHICH KKK IS DESIRED
GKKK: DEC R0
MUL #28.,R0 ;POINT TO I,I ELEMENT OF MATRIX
LDF KTH(R0),AC2 ;GET IT
RTS PC
;DATA FOR STIFFNESS SERVO SYSTEM
DATA
STFDAT: .WORD 0 ;STATUS OF STIFFNESS SYSTEM
KC: .FLT2 -50.0 ;CARTESIAN STIFFNESS MATRIX (DIAGONAL)
.FLT2 -50.0
.FLT2 -50.0
.FLT2 -10000.0
.FLT2 -10000.0
.FLT2 -500.0
BIASJA: JC ;POINTER TO JACOBIAN TO BIAS FORCE FRAME
BLKPTR: .WORD 0 ;POINTS TO DEVICE BLOCK
KSRV: .WORD 0 ;POINTS TO CURRENT SERVO POINTER IN DEV. BLK.
KSVEVT: .WORD 0 ;EVENT
KPASS: .word 0 ;KSRVO PASS COUNTER
KFTTIM: .FLT2 40.0 ;TOTAL SEGMENT TIME IN TICKS→2/3 SEC.
KPTIME: .WORD 0 ;PRESENT TIME INTO SEGEMET IN TICKS
KFPTIM: .FLT2 0.0 ;F.P. PTIME
KELTIM: .FLT2 16.0 ;ELAPSED TIME BETWEEN SERVOING IN msec
TC: .FLT2 0.0 ;STORAGE FOR COMMANDED TORQUE
ALPHA: .FLT2 0.78
BETA: .FLT2 0.07
TDBD: .FLT2 30.0 ;1 OZ AT 30 IN. DEADBAND
;FILTER PARAMETERS
AA: .FLT2 31.4 ;5*6.283 = 5 HZ ZERO
BB: .FLT2 157.0 ;25.0*6.283 = 25 HZ POLE
SPT: .FLT2 0.016 ;SEC/TIC (CURRENTLY SEC/SERVO PERIOD)
;DATA STORAGE FOR STIFFNESS SYSTEM MATRICES
JTCAL: .BLKW 96. ;STORAGE FOR JOINT TORQUE CALIBRATION MAT.
JC: .FLT2 1.0,0.0,0.0,0.0,0.0,0.0 ;6x6 FLOATING PT. JACOBIAN MATRIX
.FLT2 0.0,1.0,0.0,0.0,0.0,0.0
.FLT2 0.0,0.0,1.0,0.0,0.0,0.0
.FLT2 0.0,0.0,0.0,1.0,0.0,0.0
.FLT2 0.0,0.0,0.0,0.0,1.0,0.0
.FLT2 0.0,0.0,0.0,0.0,0.0,1.0
KCJCOL: KCJ ;POINTER TO COLUMN OF KCJ
KCJ: .BLKW 36.*2 ;STORAGE FOR INTERMEDIATE PRODUCT
KTH: .BLKW 36.*2 ;STORAGE FOR STIFFNESS MATRIX (COL MAJ)
JPRIME: .BLKW 36.*2 ;STORAGE FOR JACOBIAN TO STIFFNESS FRAME,C
RELX: .WORD 0
.FLT2 1.0,2.0,3.0 ;mem test
C66: .FLT2 1.0,0.0,0.0,0.0,0.0,0.0 ;6*6 REL XFORM FROM HAND TO C
.FLT2 0.0,1.0,0.0,0.0,0.0,0.0
.FLT2 0.0,0.0,1.0,0.0,0.0,0.0
.FLT2 0.0,0.0,0.0,1.0,0.0,0.0
.FLT2 0.0,0.0,0.0,0.0,1.0,0.0
.FLT2 0.0,0.0,0.0,0.0,0.0,1.0
;END OF STIFFNESS DATA AREA
;OPERATE ROUTINE TO SETUP SCREWDRIVER OR VISE OPERATION
;
;THIS ROUTINE WILL INITIATE THE VISE OR SCREWDRIVER OPERATION. R0 MUST
;POINT TO A COEFFICENT BLOCK AS DESCRIBED BELOW. R1 POINTS TO STORAGE FOR
;DEVICE BLOCK (5 WORDS). THE VISE CAN BE MADE TO STOP ON CONTACT BY SETTING
;THE NNUL BIT IN THE MODBITS OF THE COEFF. LIST. THE CONTACT FORCE WILL DEPEND
;UPON THE STOPWAIT TIME GIVEN. IF THE SCREW DRIVER IS GIVEN BOTH TORQUE AND
;VELOCITY COMMANDS THE TORQUE COMMAND WILL BE FOLLOWED. TORQUE COMMAND MUST
;BE ZERO FOR VELOCITY SERVOING TO OCCUR. THE SCREWDRIVER VARIABLE TH WILL
;CONTAIN THE NUMBER OF DEGREES TURNED SINCE STARTING TO TURN AND IS RESET AT
;THE BEGINNING OF SCREWDRIVER OPERATION.
;
;THE ROUTINE RETURNS AFTER THE DEVICE IS RUN WITH AN ERROR CODE IN R0
;IF THERE WERE ANY RUN ERRORS.
;
;SAMPLE CALL:
; MOV #COFLST,R0
; MOV #DEVICE,R1
; JSR PC,OPERATE
; TST R0
; BMI ERROR
;
;THE COEFFICENT LIST IS AS FOLLOWS:
;
;(R0)→ 1 ;SERVO BITS: 2 FOR VISE, 1 FOR DRIVER
; 0 ;SERVO BITS
; 0 ;MODBITS 1→no nulling, 0→exactly
; 0 ;WOBPTR
; 0 ;RELSEG PTR
; 250 ;TIME IN TICKS/20
; 0.0 ;TORQUE IN OZ-IN FOR DRIVER, STOPWAIT TIME IN MSEC FOR VISE (FP #)
; 100.0 ;VELOCITY IN DEG/MSEC FOR DRIVER, POSITION GOAL (IN) FOR VISE (FP #)
;CHANGE WHERE TO PASS BACK TH FOR DRIVER AND VISE
CODE
OPERATE:MOV R2,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R4,-(SP)
MOV R5,-(SP)
MOV R1,-(SP) ;SAVE DEV POINTER
MOV R1,OPRDEV ;
MOV R0,OPRBLK ;SAVE POINTER TO COEF. DATA LIST IN INFO BLK
MOV #OPRBLK,R0 ;POINT TO SERVO DATA
JSR PC,ATTSRV ;ATTACH SERVOS TO THIS DEVICE
BCC OPINIT ;BRANCH IF NO ATTACH ERRORS
;SET UP RUN PARAMETERS
OPINIT: MOV (SP),R0 ;POINT TO DEV BLK
CMP (R0)+,(R0)+
MOV (R0),DAT ;GET SERVO POINTER
MOV OPRBLK,R1 ;POINT TO OPERATE COEFF LIST
ADD #12,R1 ;POINT TO OPERATE TIME
MOV (R1)+,R3
CLR R2
DIV #20.,R2 ;CHANGE TO 40 MSEC TICKS
MOV R2,PTIME(DAT) ;SET RUNTIME
LDF (R1)+,AC0 ;SET FORCE LEVEL OR STOP WAIT TIME
STF AC0,FLEVEL(DAT)
LDF (R1)+,AC0 ;SET VELOCITY OR POSITION
BITB #SCRE,MECHSM(DAT)
BEQ 1$ ;BR IF NOT SCREWDRIVER
STF AC0,THDP(DAT) ;SET VELOCITY GOAL
CLRF TH(DAT) ;0 DEGREES TURNED
CLRF THD(DAT) ;0 INITIAL VELOCITY
CLRB DACINF(DAT) ;ZERO INITIAL OUTPUT
BR DVSCH
1$: BITB #VIS,MECHSM(DAT)
BEQ 3$ ;BR IF NOT VISE
MOV #NOSOLU,R0
CMPF USTOP(DAT),AC0 ;CHECK IF IN RANGE
CFCC
BLE OPRDNE
CMPF LSTOP(DAT),AC0
CFCC
BGE OPRDNE
STF AC0,THP(DAT) ;SET POSITION GOAL
BR DVSCH
3$: MOV #WRGNUM,R0 ;MUST NOT BE VISE|SCREWDRIVER
BR OPRDNE
;RUN THE DEVICE REQUESTED
DVSCH: EVMAK ;THIS EVENT SIGNALS SERVOS DONE
MOV 2(SP),R1 ;GET ADDRESS OF DEVICE BLOCK
TST (R1)+ ;POINT TO SPACE FOR EVENT
MOV (SP),(R1)+ ;SAVE EVENT NUMBER IN DEVICE BLOCK
SCHEDU #OPRPDB,#OPRLV6,#USRDM,#20
1$: EVWAIT (SP) ;WAIT FOR SERVOS TO FINISH
EVKIL ;DELETE EVENT NUMBER
CLR R0 ;NO ERRORS
OPRDNE: MOV (SP)+,R1 ;RELEASE SERVOS IN DEVICE BLOCK
JSR PC,RELSRV
MOV (SP)+,R5 ;RESTORE REGISTERS
MOV (SP)+,R4
MOV (SP)+,R3
MOV (SP)+,R2
RTS PC
;OPRLV6 ROUTINE TO OPERATE SCREWDRIVER OR VISE
OPRLV6: MOV OPRDEV,R0 ;POINT TO DEV BLOCK
MOV 4(R0),DAT ;POINT TO SERVO BLOCK
TST @INUSE(DAT) ;CHECK IF SOMEONE ELSE SAYS TO DIE
BMI OP6DNE
TST (DAT) ;DONE YET?
BMI OP6DNE
DEC PTIME(DAT) ;DECREMENT TIME
BLE OP6DNE
1$: BIT #1,PTIME(DAT)
BEQ 4$
JSR PC,READ ;READ VELOCITY
SUB TACHZ0(DAT),R0 ;SUBTRACT TAC ZERO READING
MOV R0,R1 ;SET TO ZERO IF IN NOISE BAND
BGE 7$
NEG R1
7$: BIC #7,R1
BNE 8$
CLR R0
8$: LDCIF R0,AC0
MULF VSCALE(DAT),AC0 ;GET VELOCITY IN DEG/MSEC
STF AC0,THD(DAT) ;AND SAVE
MULF KI(DAT),AC0 ;GET DEGREES TURNED THIS PERIOD DEG/MSEC*MSEC=DEG
ADDF TH(DAT),AC0 ;INTEGRATE TOTAL DEGREES TURNED SINCE START
STF AC0,TH(DAT)
LDF FLEVEL(DAT),AC1 ;ARE WE FORCE SERVOING?
CFCC
BEQ 13$
LDF AC1,AC0
BR 12$
13$: LDF THDP(DAT),AC0
ADDF THD(DAT),AC0 ;COMPUTE ERROR VEL
MULF KV(DAT),AC0 ;*VELOCITY GAIN
12$: CFCC
BLT 5$
ADDF V0(DAT),AC0 ;ADD IN FRICTION TORQUE
BR 6$
5$: SUBF V0(DAT),AC0
6$: MULF MSCALE(DAT),AC0 ;CONVERT TO DAC UNITS
STCFI AC0,R0 ;GET INTEGER DRIVE VALUE
MOV MAXDRV(DAT),R2 ;GET MAX DRIVE IN CASE WE SATURATE
MOV R0,R1 ;CHECK FOR EXCESSIVE TORQUE
BGE 2$
NEG R1 ;GET ABS VALUE OF DRIVE
NEG R2 ;NEG MAX DRIVE
2$: CMP R1,MAXDRV(DAT)
BLE 3$
MOV R2,R0 ;USE MAX DRIVE
DEC COUNT(DAT) ;COUNT SATURATION TIME
BGE 3$
BIS #4000,(DAT) ;SATURATION OCCURED TO LONG
BR OP6DNE ;SO QUIT
3$: BIC #177700,R0 ;REMOVE EXTRA SIGN BITS IF ANY
BIS R0,DACINF(DAT) ;OR IN CHANNEL AND SENSE BIT
BIC #200,DACINF(DAT) ;ENABLE SCR DRIVE
MOV #40,DR11S ;SET MODE
MOV DACINF(DAT),DR11O ;SEND OUTPUT TO INTERFACE
SLEEP #32. ;LET IT RUN FOR A WHILE
BR OPRLV6
4$: MOV #40,DR11S ;SET MODE
BIS #200,DACINF(DAT) ;DISABLE DRIVE
MOV DACINF(DAT),DR11O ;ZERO DRIVE FOR THIS PASS
SLEEP #8.
BR OPRLV6
OP6DNE: BIC #3,DR11S ;SET MODE AND DISABLE INTERUPT
MOV #160000,DR11O ;SET ZERO DRIVE
MOV OPRDEV,R0
EVSIG 2(R0) ;SIGNAL DONE *K
DISMIS
READ: MOV #42,DR11S ;CODE TO READ A/D
MOVB TACCHN(DAT),DR11O
TSTB DR11S ;WAIT TIL DONE
BPL .-4
MOV DR11I,R0
SUB #3777,R0 ;OFFSET SO 0=0
RTS PC
;OPERATE DATA
DATA
OPRDEV: .WORD 0 ;POINTER TO DEV. BLOCK
OPRBLK: 0 ;POINTER TO COEF. DATA LIST
100. ;ALLOW 4 SECOND OVERLOAD ON MOTOR
0 ;NO START DELAY
0 ;NO POLYNOMIALS
;OPERATE PDB BLOCK
OPRPDB: PDBLK 6,20
;VARIABLES, CONSTANTS, AND TEMPORARY STORAGE AREA COMMON TO ALL SERVOS
DATA
VELERR: .FLT2 0.0 ;VELOCITY ERROR, THETA DOT - THETA DOT PREDICTED
TORQUE: .BLKW 2 ;OUTPUT MOTOR TORQUE
A05TH: .FLT2 0.0 ;5TH ORDER CORRECTION EQUATION
A15TH: .FLT2 0.0
A25TH: .FLT2 0.0
A35TH: .FLT2 10.0
A45TH: .FLT2 -15.0
A55TH: .FLT2 6.0
FP6: .FLT2 6.0
FP10: .FLT2 10.0
FP15: .FLT2 15.0
FP30: .FLT2 30.0
FP60: .FLT2 60.0
FP90: .FLT2 90.0
ONE48T: .FLT2 .020833333
ONE32N: .FLT2 .03125
;SINUSOIDAL TABLE FOR JOINT WOBBLING
WOBTAB: .FLT2 .0000000, .3826834, .7071068, .9238795
.FLT2 1.000000, .9238795, .7071068, .3826834
.FLT2 .0000000,-.3826834,-.7071068,-.9238795
.FLT2 -1.000000,-.9238795,-.7071068,-.3826834
YWOBMG: .FLT2 0.0 ;MAGNITUDE OF YELLOW ARM WOBBLE
BWOBMG: .FLT2 0.0 ; " " BLUE " "
SRV01: 0 ;YELLOW ARM JOINT #1 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 1 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 140511,7733 ;TH -Pi
.WORD 140511,7733 ;THP -Pi
.WORD 140511,7733 ;THN -Pi
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.FLT2 300.0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 2.0 ;KKI
.FLT2 -.0005 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -1000.0 ;KKK
.FLT2 1.0 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
0 ;POTCHN see armdoctxt[armted] *k
8. ;TACCHN ditto
DR11O ;DACADD
.BYTE 1 ;DACCHN
.BYTE 0 ;DACVAL
401 ;DACINF
.FLT2 .219 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.FLT2 1.0 ;SCALE
.FLT2 0.0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV01,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV02: 0 ;YELLOW ARM JOINT #2 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 2 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 140311,7733 ;TH -Pi/2
.WORD 140311,7733 ;THP -Pi/2
.WORD 140311,7733 ;THN -Pi/2
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.FLT2 500. ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
1 ;POTCHN
9. ;TACCHN
DR11O ;DACADD
.BYTE 2 ;DACCHN
.BYTE 0 ;DACVAL
1002 ;DACINF
.FLT2 .175 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.FLT2 1.0 ;SCALE
.FLT2 0.0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV02,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV03: 0 ;YELLOW ARM JOINT #3 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 3 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 41100,0 ;TH 3.0 inches
.WORD 41100,0 ;THP 3.0 inches
.WORD 41100,0 ;THN 3.0 inches
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.FLT2 30.0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
2 ;POTCHN
10. ;TACCHN
DR11O ;DACADD
.BYTE 3 ;DACCHN
.BYTE 0 ;DACVAL
2004 ;DACINF
.FLT2 2.28 ;MSCALE
.WORD 40200, 0 ;TOTQE 1.000000
.WORD 0 ;DITHER
.FLT2 1.0 ;SCALE
.FLT2 0.0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS multiple wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV03,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV04: 0 ;YELLOW ARM JOINT #4 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 4 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 140311,7733 ;TH -Pi/2
.WORD 140311,7733 ;THP -Pi/2
.WORD 140311,7733 ;THN -Pi/2
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.FLT2 40.0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCII
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
3 ;POTCHN
-1. ;TACCHN
DR11O ;DACADD
.BYTE 4 ;DACCHN
.BYTE 0 ;DACVAL
4010 ;DACINF
.FLT2 2.38 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.FLT2 1.0 ;SCALE
.FLT2 0.0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV04,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV05: 0 ;YELLOW ARM JOINT #5 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 5 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 40311,7733 ;TH Pi/2
.WORD 40311,7733 ;THP Pi/2
.WORD 40311,7733 ;THN Pi/2
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.FLT2 50.0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
4 ;POTCHN
-1 ;TACCHN none
DR11O ;DACADD
.BYTE 5 ;DACCHN
.BYTE 0 ;DACVAL
10020 ;DACINF
.FLT2 2.87 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.FLT2 1.0 ;SCALE
.FLT2 0.0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS multiple wipers
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV05,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV06: 0 ;YELLOW ARM JOINT #6 DATA
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 6 ;SRVNUM
.BYTE YELARM ;MECHSM
.WORD 0,0 ;TH 0 degrees
.WORD 0,0 ;THP 0 degrees
.WORD 0,0 ;THN 0 degrees
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.FLT2 70.0 ;V0
.WORD 0,0 ;CI
.BLKW 2 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.BLKW 2 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
5 ;POTCHN
-1 ;TACCHN none
DR11O ;DACADD
.BYTE 6 ;DACCHN
.BYTE 0 ;DACVAL
20040 ;DACINF
.FLT2 3.33 ;MSCALE
.WORD 36616,175065 ;TOTQE .1745329@-1
.WORD 0 ;DITHER
.FLT2 1.0 ;SCALE
.FLT2 0.0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV06,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV07: 0 ;YELLOW ARM HAND SERVO DATA BLOCK
.IFZ NOYELW
0 ;MODBTS
0 ;INUSE
.BYTE 7 ;SRVNUM
.BYTE YELHND ;MECHSM
.WORD 40500,0 ;TH 3.0 inches
.WORD 40500,0 ;THP 3.0 inches
.WORD 40500,0 ;THN 3.0 inches
.WORD 0,0 ;THD
.WORD 0,0 ;THDP
.WORD 0,0 ;THDN
.WORD 0,0 ;DELTH
.WORD 0,0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
YWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.WORD 0,0 ;FTTIME
0 ;PTIME
.WORD 0,0 ;ELTIME
0 ;COUNT
.WORD 0,0 ;ERRINT
.WORD 0,0 ;ERRTQE
.WORD 0,0 ;ERRTOL
.WORD 0,0 ;KV
.WORD 0,0 ;KE
.WORD 0,0 ;KI
.FLT2 30.0 ;V0
.WORD 0,0 ;CI
.WORD 0,0 ;DCI
.WORD 0,0 ;NCI
.WORD 0,0 ;CII
.WORD 0,0 ;DCII
.WORD 0,0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .5 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -20.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
7 ;POTCHN
-1 ;TACCHN none
DR11O ;DACADD
.BYTE 7 ;DACCHN
.BYTE 0 ;DACVAL
100 ;DACINF no tach feedback
.FLT2 .178 ;MSCALE
.WORD 40200, 0 ;TOTQE 1.000000
.WORD 0 ;DITHER
.FLT2 1.0 ;SCALE
.FLT2 0.0 ;OFFSET
.WORD 0,0 ;USTOP
.WORD 0,0 ;LSTOP
.WORD 0,0 ;STPBND
.WORD 0,0 ;VSCALE
177 ;MAXDRV
0 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV07,20 ;PROCESS DESCRIPTOR BLOCK
.ENDC
SRV08: 0 ;BLUE ARM JOINT #1 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 8. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 180.0 ;TH
.FLT2 180.0 ;THP
.FLT2 180.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 .10 ;ERRTOL
.FLT2 -.0320 ;KV -.05
.FLT2 -125000.0 ;KE -150000.0
.FLT2 -5.854@11 ;KI -.6@12
.FLT2 222.0 ;V0 600.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1.0@6 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 5.0 ;KKI
.FLT2 .03 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -300.0 ;KKK -1000.0
.FLT2 2.0 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
1000. ;POT
0 ;TACH
21 ;POTCHN
20 ;TACCHN
174000 ;DACADD
.BYTE 1 ;DACCHN
.BYTE 0 ;DACVAL
401 ;DACINF
.FLT2 .146092@-1 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE (pi/180)
.WORD 0 ;DITHER
.FLT2 -.08653846 ;SCALE
.FLT2 259.5289 ;OFFSET
.FLT2 190.00 ;USTOP
.FLT2 -45.00 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -1.6570@-4 ;VSCALE
177 ;MAXDRV
10. ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV08,20 ;PROCESS DESCRIPTOR BLOCK
SRV09: 0 ;BLUE ARM JOINT #2 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 9. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 -90.0 ;TH
.FLT2 -90.0 ;THP
.FLT2 -90.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.10 ;ERRTOL
.FLT2 -.0333 ;KV -.06
.FLT2 -1.25@5 ;KE -1.25@5
.FLT2 -9.0@11 ;KI -9.0@11
.FLT2 400.0 ;V0 600.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1.44@6 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 10.0 ;KKI
.FLT2 .008 ;KKV
.FLT2 -90.0 ;CTH0
.FLT2 -100.0 ;KKK -1000.0
.FLT2 1.0 ;KKF 4.0 3.5 3.0 1.5
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
14 ;POTCHN
10 ;TACCHN
174000 ;DACADD
.BYTE 2 ;DACCHN
.BYTE 0 ;DACVAL
1002 ;DACINF
.FLT2 .5353@-2 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .06686479 ;SCALE
.FLT2 -222.9941 ;OFFSET
.FLT2 -50.00 ;USTOP
.FLT2 -165.00 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -1.5401@-4 ;VSCALE
177 ;MAXDRV
10. ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV09,20 ;PROCESS DESCRIPTOR BLOCK
SRV10: 0 ;BLUE ARM JOINT #3 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 10. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 12.0 ;TH inches
.FLT2 12.0 ;THP inches
.FLT2 12.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.02 ;ERRTOL
.FLT2 -0.0537 ;KV -0.03
.FLT2 -500.0 ;KE -200.0
.FLT2 -1.86@7 ;KI -4.0@6
.FLT2 5.0 ;V0 12.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 484940.0 ;CII
.BLKW 2 ;DCII
.FLT2 484940.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .50 ;KKI
.FLT2 .20 ;KKV
.FLT2 14.0 ;CTH0
.FLT2 -100.0 ;KKK
.FLT2 1.0 ;KKF 2.0
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
.BLKW 1 ;POT
.BLKW 1 ;TACH
15 ;POTCHN
11 ;TACCHN
174000 ;DACADD
.BYTE 3 ;DACCHN
.BYTE 0 ;DACVAL
2004 ;DACINF
.FLT2 -0.43478 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .9099181@-2 ;SCALE
.FLT2 .8012 ;OFFSET
.FLT2 33.00 ;USTOP
.FLT2 6.75 ;LSTOP
.FLT2 1.0 ;STPBND
.FLT2 3.0457@-5 ;VSCALE
177 ;MAXDRV
1 ;TACHZ0
0 ;WIPERS single
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV10,20 ;PROCESS DESCRIPTOR BLOCK
SRV11: 0 ;BLUE ARM JOINT #4 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 11. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 -90.0 ;TH
.FLT2 -90.0 ;THP
.FLT2 -90.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.2 ;ERRTOL
.FLT2 -.03 ;KV was -0.0448
.FLT2 -15000.0 ;KE -7000.0
.FLT2 -3.08@9 ;KI -3.0@9
.FLT2 19.0 ;V0 70.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1.0@4 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 10.0 ;KKI
.FLT2 .01 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -200.0 ;KKK
.FLT2 .2 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
2 ;POTCHN
6 ;TACCHN
174000 ;DACADD
.BYTE 4 ;DACCHN
.BYTE 0 ;DACVAL
4010 ;DACINF
.FLT2 .9924@-1 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .0925687 ;SCALE
.FLT2 -157.569406 ;OFFSET
.FLT2 205.00 ;USTOP
.FLT2 -295.00 ;LSTOP
.FLT2 10.0 ;STPBND
.FLT2 -4.0894@-4 ;VSCALE
177 ;MAXDRV
56 ;TACHZ0 was 51.
MWIP11 ;WIPERS multiple wipers
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV11,20 ;PROCESS DESCRIPTOR BLOCK
.WORD 0,0,0,0,0 ;MULT. WIPER DATA
2 ;A/D CHAN WIPER #1
.FLT2 -518.07277 ;OFFSET
.FLT2 .0925687 ;SCALE
13 ;A/D CHAN WIPER #2
.FLT2 -322.35 ;OFFSET WAS -327.35677
.FLT2 .0870195 ;SCALE
MWIP11: 2 ;A/D CHAN WIPER #1
.FLT2 -157.569406 ;OFFSET
.FLT2 .0925687 ;SCALE
13 ;A/D CHAN WIPER #2
.FLT2 38.18 ;OFFSET WAS 32.186366
.FLT2 .0870195 ;SCALE
0
SRV12: 0 ;BLUE ARM JOINT #5 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 12. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 90.0 ;TH
.FLT2 90.0 ;THP
.FLT2 90.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.1 ;ERRTOL
.FLT2 -0.0242 ;KV -0.04
.FLT2 -15000.0 ;KE -2000.0
.FLT2 -1.34@9 ;KI -1.0@9
.FLT2 15.0 ;V0 60.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 0.0 ;KKI *K WAS +.1!
.FLT2 .02 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -400.0 ;KKK
.FLT2 .1 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
3 ;POTCHN
7 ;TACCHN
174000 ;DACADD
.BYTE 5 ;DACCHN
.BYTE 0 ;DACVAL
10020 ;DACINF
.FLT2 -.1299540 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
.WORD 0 ;DITHER
.FLT2 .6711409@-1 ;SCALE
.FLT2 -136.9799 ;OFFSET
.FLT2 95.00 ;USTOP
.FLT2 -95.00 ;LSTOP
.FLT2 5.0 ;STPBND
.FLT2 -3.7919@-4 ;VSCALE
177 ;MAXDRV
23. ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV12,20 ;PROCESS DESCRIPTOR BLOCK
SRV13: 0 ;BLUE ARM JOINT #6 DATA
0 ;MODBTS
0 ;INUSE
.BYTE 13. ;SRVNUM
.BYTE BLUARM ;MECHSM
.FLT2 0.0 ;TH
.FLT2 0.0 ;THP
.FLT2 0.0 ;THN
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD 0 ;WOBPTR
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.7 ;ERRTOL
.FLT2 -0.1375 ;KV -0.1
.FLT2 -800.0 ;KE -1000.0
.FLT2 -5.0@7 ;KI -2.18@8
.FLT2 9.0 ;V0 25.0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 0.0 ;CII
.BLKW 2 ;DCII
.FLT2 0.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .05 ;KKI
.FLT2 .01 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -10.0 ;KKK
.FLT2 2.0 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
4 ;POTCHN
-1 ;TACCHN none
174000 ;DACADD
.BYTE 6 ;DACCHN
.BYTE 0 ;DACVAL
20040 ;DACINF
.FLT2 0.075188 ;MSCALE
.FLT2 .1745329@-1 ;TOTQE
5. ;DITHER
.FLT2 -.850260@-1 ;SCALE
.FLT2 223.023 ;OFFSET
.FLT2 200.00 ;USTOP
.FLT2 -110.00 ;LSTOP
.FLT2 5.0 ;STPBND
.BLKW 2 ;VSCALE no tachometer
177 ;MAXDRV
.BLKW 1 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV13,20 ;PROCESS DESCRIPTOR BLOCK
SRV14: 0 ;BLUE ARM HAND SERVO DATA BLOCK
0 ;MODBTS
0 ;INUSE
.BYTE 14. ;SRVNUM
.BYTE BLUHND ;MECHSM
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 16.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.1 ;ERRTOL
.FLT2 -0.06 ;KV
.FLT2 -800.0 ;KE
.FLT2 -0.1@8 ;KI
.FLT2 30.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1590000.0 ;CII
.BLKW 2 ;DCII
.FLT2 1590000.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .005 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 -100.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
5 ;POTCHN
-1 ;TACCHN none
174000 ;DACADD
.BYTE 7 ;DACCHN
.BYTE 0 ;DACVAL
100 ;DACINF no tach feedback
.FLT2 0.10593 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 -.1358081@-2 ;SCALE
.FLT2 4.344500 ;OFFSET
.FLT2 3.80 ;USTOP
.FLT2 -1.00 ;LSTOP was -.2
.FLT2 0.1 ;STPBND
.BLKW 2 ;VSCALE none
177 ;MAXDRV
.BLKW 1 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV14,20 ;PROCESS DESCRIPTOR BLOCK
SRV15: 0 ;VISE SERVO DATA BLOCK
0 ;MODBTS
0 ;INUSE
.BYTE 15. ;SRVNUM
.BYTE VIS ;MECHSM
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 40.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 0.50 ;ERRTOL
.FLT2 -0.06 ;KV
.FLT2 -800.0 ;KE
.FLT2 -0.1@8 ;KI
.FLT2 30.0 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1590000.0 ;CII
.BLKW 2 ;DCII
.FLT2 1590000.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .005 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 1.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
28. ;POTCHN
-1 ;TACCHN none
DR11O ;DACADD
.BYTE 10 ;DACCHN *K
.BYTE 0 ;DACVAL
100 ;DACINF no tach feedback
.FLT2 0.10593 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 1.4432@-3 ;SCALE
.FLT2 -.748 ;OFFSET
.FLT2 4.6875 ;USTOP
.FLT2 -0.10 ;LSTOP
.FLT2 0.1 ;STPBND
.BLKW 2 ;VSCALE none
0 ;MAXDRV
.BLKW 1 ;TACHZ0
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV15,20 ;PROCESS DESCRIPTOR BLOCK *k
SRV16: 0 ;SCREWDRIVER SERVO DATA BLOCK
0 ;MODBTS
0 ;INUSE
.BYTE 16. ;SRVNUM
.BYTE SCRE ;MECHSM
.FLT2 3.0 ;TH inches
.FLT2 3.0 ;THP inches
.FLT2 3.0 ;THN inches
.FLT2 0.0 ;THD
.FLT2 0.0 ;THDP
.FLT2 0.0 ;THDN
.FLT2 0.0 ;DELTH
.FLT2 0.0 ;DELTHD
.FLT2 0.0 ;POSERR
.WORD -1 ;WOBPTR don't wobble
BWOBMG ;WOBMAG
.BLKW 1 ;DATPT
.BLKW 1 ;POLY
.BLKW 1 ;FCI
16. ;SDTIME
0 ;TTIME
.FLT2 0.0 ;FTTIME
0 ;PTIME
.FLT2 40.0 ;ELTIME
0 ;COUNT
.FLT2 0.0 ;ERRINT
.FLT2 0.0 ;ERRTQE
.FLT2 1.0@8 ;ERRTOL
.FLT2 .01 ;KV
.FLT2 -800.0 ;KE
.FLT2 40.0 ;KI
.FLT2 4.25 ;V0
.FLT2 0.0 ;CI
.BLKW 2 ;DCI
.FLT2 0.0 ;NCI
.FLT2 1590000.0 ;CII
.BLKW 2 ;DCII
.FLT2 1590000.0 ;NCII
.FLT2 0.0 ;FLEVEL
.FLT2 .005 ;KKI
.FLT2 -1000.0 ;KKV
.FLT2 90.0 ;CTH0
.FLT2 1.0 ;KKK
.FLT2 .05 ;KKF
.FLT2 0.0 ;YOLD
.FLT2 0.0 ;UOLD
0 ;POT
0 ;TACH
-1 ;POTCHN NONE
29. ;TACCHN DRIVER BACK EMF
DR11O ;DACADD YELLOW DAC CONTROL REG
.BYTE 10 ;DACCHN *K
.BYTE 0 ;DACVAL
160000 ;DACINF YELLOW INTERFACE DAC CHAN
.FLT2 1.34 ;MSCALE
.FLT2 1.0 ;TOTQE
.WORD 0 ;DITHER
.FLT2 -.1358081@-2 ;SCALE
.FLT2 4.344500 ;OFFSET
.FLT2 3.80 ;USTOP
.FLT2 -0.20 ;LSTOP
.FLT2 0.1 ;STPBND
.FLT2 -.0025 ;VSCALE
37 ;MAXDRV
21 ;TACHZ0 was 1
0 ;WIPERS single wiper
.IFZ ISLIN
.BYTE 0,0,0,0 ;NONLIN
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0,0,0
.BYTE 0,0
.ENDC
PDBLK7 SERVO,SRV15,20 ;PROCESS DESCRIPTOR BLOCK *k
;DIAGNOSTIC DATA BUFFER: ORGANIZATION AND LOCAL STORAGE
;THE DIAGNOSTIC DATA ARRAY IS OF THE FOLLOWING FORM
;
; DBUF: LAST WORD
; +2 : PTIME ;TIME SINCE THE START OF CURRENT SEGMENT(MSEC)
; +4 : TH|POSERR ;JOINT ANGLE OR ANGLE ERROR(F.P.) IF ACTUAL 1|0
; +10 : THD|VELERR ;JOINT VELOCITY OR VELOCITY ERROR (F.P.)
; +14 : DACOUT ;OUTPUT MOTOR DAC
;
;THE SEQUENCE REPEATS STARTING WITH SRVNUM AGAIN. THE POINTER "DBUF" IS
;ALWAYS LEFT POINTING AT THE FIRST UNUSED WORD IN THE DIAGNOSTIC BUFFER.
DBUF:
.IFNZ DIAGY
.+2 ;STORAGE AREA FOR DIAGNOSTIC JOINT DATA
.BLKW DBUFL
ACTUAL: 0 ;SET 0 = SEND ERROR TERMS, 1 = ACTUAL ADC READINGS
.ENDC
.IFNZ TACCAL
BELL: .BYTE 7,0
.ENDC
.IFNZ TIMER
TIMES: .BLKW 1000. ;BUFFER AREA TO SAVE SERVO EXECUTION TIME
.ENDC
.IFNZ FRCDAT
FDATA: .BLKW 2000. ;FORCE SENSING SYSTEM DATA BUFFER AREA
.ENDC
GIAREA: .BLKW 4. ;INTEGER STORAGE FOR GATHER
GAREA: 0 ; changed to 0 by MSM
READW: 0 ; changed to 0 by MSM
ARMEND: